vector<vector<int> > kmeansModule::getClusters(float** data, int M, vector <int>& pts) { int N = pts.size(); vector<vector<int> > clusters (K, vector<int>(0, 0)); for (int i = 0; i < N; i++) clusters[clusterMemberships[i]].push_back(pts[i]); return clusters; }
std::size_t FilterGraph<Graph>::numClusters() const { int c=0; auto it = clusters(); for(; it.first != it.second; ++it.first) ++c; return c; };
std::vector<std::vector<int>> clusters_from_labels(const std::vector<int>& labels, unsigned int cluster_count) { std::vector<std::vector<int>> clusters(cluster_count); for(unsigned int i=0; i<labels.size(); i++) { int label = labels[i]; if(label >= 0) { clusters[label].push_back(i); } } return clusters; }
// represents and OR search free for AStar SearchTree train_search_tree_agglomerative(map<string,NNTemplateType>&allTemplates) { SearchTree search_tree_root; // initiailize the bottom layer unordered_map<int, unordered_map<int,double> > costs; list<SearchTree > clusters(allTemplates.size()); function<void (SearchTree&node)> update_dists_for_node = [&](SearchTree&node) { static atomic<int> completed(0); log_once(printfpp("++computed distance %d",completed++)); for(auto & node_k : clusters) { double cost = node.Templ.merge_cost(node_k.Templ); static mutex m; lock_guard<mutex> l(m); costs[node.id][node_k.id] = cost; costs[node_k.id][node.id] = cost; } }; auto iter = allTemplates.begin(); for(int cter = 0; iter != allTemplates.end(); ++cter, ++iter) { auto jter = clusters.begin(); std::advance(jter,cter); SearchTree&curTree = *jter; curTree.Templ = iter->second; curTree.id = seq_id++; curTree.pose_uuid = iter->first; } TaskBlock compute_init_dists("compute_init_dists"); log_once(printfpp("++Computing Initial Distances for %d clusters",(int)clusters.size())); for(auto & curTree : clusters) { SearchTree*tp = &curTree; compute_init_dists.add_callee([&,tp]() { update_dists_for_node(*tp); }); } compute_init_dists.execute(); log_once(printfpp("--Computing Initial Distances for %d clusters",(int)clusters.size())); // merge layers until we get the root merge_clusters_agglom(seq_id,clusters,costs,update_dists_for_node); // store the root search_tree_root = clusters.front(); // print the search tree log_search_tree(search_tree_root); return search_tree_root; }
vector< vector<u32> > kmeans(const vector< vector<f64> >& points, u32 k, vector< vector<f64> >& centers) { if (points.size() == 0) throw eInvalidArgument("There must be points to cluster!"); if (points[0].size() == 0) throw eInvalidArgument("There must be at least one dimension!"); for (size_t i = 1; i < points.size(); i++) if (points[i].size() != points[0].size()) throw eInvalidArgument("All data points must have the same dimensionality."); if (k == 0) throw eInvalidArgument("Clustering into zero clusters makes no sense."); if ((u32)centers.size() != k) throw eInvalidArgument("You must supply k initial centers to this version of kmeans()."); for (u32 i = 0; i < k; i++) if (centers[i].size() != points[0].size()) throw eInvalidArgument("All initial centers must have the same dimensionality as the data points."); vector< vector<u32> > clusters(k); while (true) { vector< vector<u32> > newClusters(k); for (size_t i = 0; i < points.size(); i++) { f64 dist = s_distanceSquared(points[i], centers[0]); u32 closest = 0; for (u32 c = 1; c < k; c++) { f64 distHere = s_distanceSquared(points[i], centers[c]); if (distHere < dist) { closest = c; dist = distHere; } } newClusters[closest].push_back((u32)i); } for (u32 i = 0; i < k; i++) if (newClusters[i].size() > 0) centers[i] = s_calcCenter(points, newClusters[i]); // Else, what should I do? Leave it? Randomize a new center? if (clusters == newClusters) break; clusters = newClusters; } return clusters; }
const cluster_t *find_nearest_cluster( const sample_type& s, int& nearest_index) const { const_iterator it( clusters().begin()); real_type min_dist = traits_type::distance( s, it->mean); nearest_index = 0; const cluster_t *cluster = &(*it); ++it; for( int index = 0; it != clusters().end(); ++it, ++index) { real_type d = traits_type::distance( s, it->mean); if( d < min_dist) { min_dist = d; nearest_index = index; cluster = &(*it); } } return cluster; }
std::vector<DataVector> sampleSelectionKMeans(std::vector<DataVector>& samples, std::size_t n, std::size_t max_iter) { if(samples.size() <= n) return samples; std::size_t dim = samples[0].dim(); std::vector<DataVector> centers = sampleSelectionInitialize(samples, n); std::size_t n_clusters = centers.size(); // clusters[i] contains the id of points in the i-th cluster std::vector<std::vector<std::size_t> > clusters(n_clusters); for(std::size_t iter = 0; iter < max_iter; ++iter) { for(std::size_t i = 0; i < samples.size(); ++i) { std::size_t min_cluster_id = -1; double min_cluster_dist = (std::numeric_limits<double>::max)(); for(std::size_t j = 0; j < centers.size(); ++j) { double dist = 0; for(std::size_t d = 0; d < dim; ++d) { double v = centers[j][d] - samples[i][d]; dist += v * v; } if(dist < min_cluster_dist) { min_cluster_dist = dist; min_cluster_id = j; } } clusters[min_cluster_id].push_back(i); } // for each cluster, compute the mean as the new center for(std::size_t i = 0; i < clusters.size(); ++i) { if(clusters[i].size() == 0) continue; DataVector mean(dim); mean.setZero(); for(std::size_t j = 0; j < clusters[i].size(); ++j) mean += samples[clusters[i][j]]; mean *= (1.0 / clusters[i].size()); centers[i] = mean; } } return centers; }
using namespace cluster int main(int argc, char* argv[]) { ClusterId num_clusters = 30; PointId num_points = 3000; Dimensions num_dimensions = 10; PointsSpace ps(num_points, num_dimensions); //std::cout << "PointSpace" << ps; Clusters clusters(num_clusters, ps); clusters.k_means(); std::cout << clusters; }
PartitionalClustering * RecursivePartitionalClusteringMetric::create_full_clustering( PartitionalClustering *center_cluster) { IMP::base::Vector<Ints> clusters(center_cluster->get_number_of_clusters()); Ints reps(clusters.size()); for (unsigned int i = 0; i < clusters.size(); ++i) { Ints outer = center_cluster->get_cluster(i); reps[i] = clustering_->get_cluster_representative( center_cluster->get_cluster_representative(i)); for (unsigned int j = 0; j < outer.size(); ++j) { Ints inner = clustering_->get_cluster(outer[j]); clusters[i].insert(clusters[i].end(), inner.begin(), inner.end()); } } IMP_NEW(internal::TrivialPartitionalClustering, ret, (clusters, reps)); validate_partitional_clustering(ret, metric_->get_number_of_items()); return ret.release(); }
std::vector<Index> GenerateClusters(Count k, Rng& rng) { std::vector<Index> clusters(g.nodeCount()); std::uniform_int_distribution<> distr_v(0, g.nodeCount()-1); auto& origs = centers_; origs.clear(); for (auto i = 0; i < k;) { auto n = distr_v(rng); auto it = lower_bound(origs.begin(), origs.end(), n); if (*it == n) { continue; } // vector... origs.insert(it, n); ++i; } return GenerateClusters(origs); }
std::vector<Index> GenerateClusters(const std::vector<Index>& origs) { std::vector<Index> clusters(g.nodeCount()); for (auto i = 0; i < origs.size(); ++i) { clusters[origs[i]] = i; } radius_.resize(centers_.size()); std::fill(radius_.begin(), radius_.end(), 0); auto proc = [&](Index to, Index from, Index edge, Value val) { clusters[to] = clusters[from]; if (val > radius_[clusters[from]]) { radius_[clusters[from]] = val; } }; WeightedBFS<EdgedGraph, Value> bfs(g, vals); bfs.runPrev(origs, proc); return clusters; }
void KMeansSlaveClient::KMeansJob(const Points& portion_of_data, const size_t K) const { const size_t portion_size = portion_of_data.size(); const size_t dimensions = portion_of_data[0].size(); std::vector<size_t> clusters(portion_size); while (true) { std::string centroids_message = Recieve(); // std::cout << "recv centroids ok" << std::endl; std::stringstream stream_with_centroids(centroids_message); Points centroids = ExtractPointsFromMessage(stream_with_centroids, K, dimensions); Points new_centroids(K, Point(dimensions)); // std::cout << " a" << centroids.size() << " " << centroids[0][2] << std::endl; std::vector<size_t> clusters_sizes(K); bool converged = JobItself(portion_of_data, centroids, clusters, new_centroids, clusters_sizes); std::stringstream converging_info_message_stream; converging_info_message_stream << converged << kComponentsDelimiter; Send(converging_info_message_stream.str()); // std::cout <<"send conv ok"<<std::endl; std::string ok_or_bye_message = Recieve(); // std::cout << "recv okbye ok: " << ok_or_bye_message << std::endl; if (ok_or_bye_message == "BYE") { break; } // std::cout <<" b"<<new_centroids[0][0]<<std::endl; std::stringstream new_centroids_and_clusters_subsizes_message; new_centroids_and_clusters_subsizes_message.precision(20); InsertPointsToMessage(new_centroids_and_clusters_subsizes_message, new_centroids.cbegin(), new_centroids.cend()); for (const auto& cluster_size : clusters_sizes) { new_centroids_and_clusters_subsizes_message << cluster_size << kComponentsDelimiter; } Send(new_centroids_and_clusters_subsizes_message.str()); // std::cout << "send new centroids ok" << std::endl; } }
vector<int> labelIntraFramePoints(vector<MotionPoint*>& points) { vector<Node<int>*> vnodes; for (int i = 0; i < points.size(); ++i) { vnodes.push_back(makeset(i)); } for (int i = 0; i < points.size(); ++i) { for (int j = i + 1; j < points.size(); ++j) { //if (i == j) continue; for (int m = 0; m < points[i]->candidates.size(); ++m) { for (int n = 0; n<points[j]->candidates.size(); ++n) { //if (points[i].GetLink(j, m, n) > .5) { if (points[i]->getLink(m, j, n) > 0.5) //probLink[j][m][n] > .5) { merge(vnodes[i], vnodes[j]); } } } } } vector<Node<int>*> labels = clusters(vnodes); vector<int> ilabels(points.size()); for (int i = 0; i < points.size(); ++i) { ilabels[i] = distance(labels.begin(), find(labels.begin(), labels.end(), findset(vnodes[i]))); } for (int i = 0; i < points.size(); ++i) { delete vnodes[i]; } return ilabels; }
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input){ sensor_msgs::PointCloud2::Ptr clusters (new sensor_msgs::PointCloud2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*input, *cloud); pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (10); ec.setMaxClusterSize (2500); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); std::vector<pcl::PointIndices>::const_iterator it; std::vector<int>::const_iterator pit; int j = 0; for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); for(pit = it->indices.begin(); pit != it->indices.end(); pit++) { //push_back: add a point to the end of the existing vector cloud_cluster->points.push_back(cloud_filtered->points[*pit]); /*cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; std::stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* j++;*/ } //Merge current clusters to whole point cloud *clustered_cloud += *cloud_cluster; } pcl::toROSMsg (*clustered_cloud , *clusters); clusters->header.frame_id = "/camera_depth_frame"; clusters->header.stamp=ros::Time::now(); pub.publish (*clusters); //sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2); //pcl::toROSMsg(*cloud_filtered, *output); //pub_plane.publish(*output); }
std::vector< std::set<idxtype> > srmclWithGraph(GraphType *graph,std::vector<std::set<idxtype> >& indices,Options opt ) { GraphType *cgraph,*t,*cgraphFinest, *cgraphCoarest; int nvtxs=graph->nvtxs; CtrlType ctrl; int levels=0; Matrix *M=NULL; int ct = ctrl.CoarsenTo = opt.coarsenTo, runMcl=0; ctrl.CType=opt.matchType; ctrl.optype=OP_KMETIS; ctrl.dbglvl=1; if ( ct < 5 ) ctrl.maxvwgt = (int) ceil( (1.5*nvtxs)/ct ); else if ( ct <= 100 ) ctrl.maxvwgt = (int) ceil( (2.0*nvtxs)/ct ); else if ( ct > 100 ) { // we can allow some imbalance here. ctrl.maxvwgt = (int) ceil( 10.0 * nvtxs/ct ); } my_AllocateWorkSpace(&ctrl,graph); cgraphCoarest = cgraph = Coarsen2Way(&ctrl,graph); FreeWorkSpace(&ctrl, graph); for(levels=0,t=cgraph ; t->finer!=NULL ; t=t->finer,levels++); int num_level = levels; printf("Coarsened %d levels\n", levels); printf("Number of vertices in coarsest graph:%d\n", cgraph->nvtxs); fflush(stdout); int times_rmcl= 1; while ( cgraph != NULL ) { Matrix *M0, *Mnew; int iters; M0=setupCanonicalMatrix(cgraph->nvtxs, cgraph->nedges, cgraph->xadj, cgraph->adjncy, cgraph->adjwgt, opt.ncutify); //do weight normalization here #ifdef TEST_OUTPUT //printf("level: %d, M0:\n",levels); //printMatrix(M0); #endif if ( cgraph->coarser == NULL ) { // if this is the coarsest graph, flow matrix is same as // the transition matrix. M=M0; // dumpMatrix(M); } if (cgraph->finer == NULL){ iters=opt.num_last_iter; cgraphFinest = cgraph; } else{ iters=opt.iter_per_level; } //printMatrix(M); Mnew=dprmcl(M,M0,cgraph, opt, iters, levels); //YK: main process! M=Mnew; if ( cgraph->finer != NULL ) //YK: map nodes to the finer graph { int nnzRefinedGraph = 2*M->nnz; if ( ctrl.CType == MATCH_POWERLAW_FC ) { int ii; nnzRefinedGraph = 0; for ( ii=0; ii<cgraph->finer->nvtxs; ii++ ) { int tx=cgraph->finer->cmap[ii]; nnzRefinedGraph += M->xadj[tx+1]-M->xadj[tx]; } } else if ( ctrl.CType == MATCH_HASH && levels <= 3 ) { int ii; nnzRefinedGraph=0; for ( ii=0; ii<cgraph->nvtxs; ii++ ) { nnzRefinedGraph += cgraph->vwgt[ii] * (M->xadj[ii+1] - M->xadj[ii]); } } // dumpMatrix(M); Mnew=propagateFlow(M,cgraph,cgraph->finer,nnzRefinedGraph); //printf("times:%d, levels:%d Mnew\n",times_rmcl, levels); //printMatrix(Mnew); if ( M != NULL ) { freeMatrix(M); } M=Mnew; } cgraph=cgraph->finer; //change to the finer graph levels--; printf("Done level %d (%d times MLR-MCL)\n", levels+1,times_rmcl); fflush(stdout); } fflush(stdout); idxtype* firstIndices = (idxtype*) malloc(sizeof(idxtype) * M->nvtxs); idxcopy(nvtxs, M->attractors, firstIndices); bool* isAttractor = (bool*) malloc(sizeof(bool) * M->nvtxs); getAttractorsForAll(M, isAttractor); int** countAttractor = (int**) malloc(sizeof(int*) * (num_level+1)); //count the number of times the node being an attractor node for(int i=0; i <= num_level ; i++){ countAttractor[i] = (int*) malloc (sizeof(int) * nvtxs); for(int j=0; j<M->nvtxs; j++){ countAttractor[i][j] = 0; } } #ifdef TEST_OUTPUT { for(int i=0; i<M->nvtxs; i++){ int level = 1, nodeIdx = i; for(cgraph = cgraphFinest; cgraph->coarser!=NULL; cgraph = cgraph->coarser, level++){ idxtype oldNodeIdx = nodeIdx; nodeIdx = cgraph->cmap[oldNodeIdx]; printf(" cmap: level:%d vid: %d to %d \n", level, oldNodeIdx+1, nodeIdx+1); } } } #endif for(int i=0; i<M->nvtxs; i++){ if(isAttractor[i]){ countAttractor[0][i]++; #ifdef TEST_OUTPUT printf("attractor vid:%d(%d times), %d-th times MLRMCL\n",i+1,countAttractor[0][i],times_rmcl);fflush(stdout); #endif int level = 1, nodeIdx = i; for(cgraph = cgraphFinest; cgraph->coarser!=NULL; cgraph = cgraph->coarser, level++){ idxtype oldNodeIdx = nodeIdx; nodeIdx = cgraph->cmap[nodeIdx]; countAttractor[level][nodeIdx]++; #ifdef TEST_OUTPUT //printf("countAttractor[%d][%d] = %d;\n", level, nodeIdx,countAttractor[level][nodeIdx]); #endif } } } //construct hash table idxtype* hashtable = idxmalloc(nvtxs,"Hashtable"); for(int i=0;i<nvtxs;i++) hashtable[i]=-1; // clear hashtable. int num_clusters = 0; //include singleton for(int i=0;i<nvtxs;i++){ if ( hashtable[firstIndices[i]] == -1){ hashtable[firstIndices[i]] = num_clusters; num_clusters++; } } /*idxtype* invHashTable = idxmalloc(num_clusters,"inverse hashtable"); //map cluster id back to the attractor id for(int i=0;i<nvtxs;i++){ if ( hashtable[firstIndices[i]] != -1){ invHashTable[hashtable[firstIndices[i]]] = firstIndices[i]; } }*/ //construct clusters std::vector< std::set<idxtype> > clusters(0); //clusters[i] contains nodes' numbers (in original graph) in cluster i for(int i=0; i<num_clusters ; i++){ std::set<idxtype> newCluster; clusters.push_back(newCluster); } for(int i=0; i<nvtxs ; i++){ if( firstIndices[i] != -1){ idxtype cID = hashtable[firstIndices[i]]; indices[i].insert(cID); if(cID == -1) continue; clusters[ cID ].insert( i ); } } idxtype* lastIndices = firstIndices; bool* lastAttractor = isAttractor; idxtype* oldHashtable = hashtable; printf("number of clusters in 1st time MLR-MCL: %d\n", num_clusters);fflush(stdout); idxtype* thisIndices = (idxtype*) malloc(sizeof(idxtype) * M->nvtxs); idxtype* newHashtable = idxmalloc(nvtxs,"newHashtable"); bool* thisAttractor = (bool*) malloc(sizeof(bool) * M->nvtxs); while(times_rmcl < opt.time_rmcl){ //do more than one times R-MCL but penalize attractor nodes (by assigning them higher inflation rate) times_rmcl++; cgraph = cgraphCoarest; for(levels=0,t=cgraph ; t->finer!=NULL ; t=t->finer,levels++); while ( cgraph != NULL ) { Matrix *M0, *Mnew; int iters; M0=setupCanonicalMatrix(cgraph->nvtxs, cgraph->nedges, cgraph->xadj, cgraph->adjncy, cgraph->adjwgt, opt.ncutify); //do weight normalization here if ( cgraph->coarser == NULL ) { M=M0; } if (cgraph->finer == NULL) iters=opt.num_last_iter; else iters=opt.iter_per_level; //Mnew=dprmcl(M,M0,cgraph, opt, iters,levels); //original Mnew=dprmcl_penalizeAttractors(M,M0,cgraph, opt, iters, levels, countAttractor); //YK: main process! M=Mnew; if ( cgraph->finer != NULL ) //YK: map nodes to the finer graph { int nnzRefinedGraph = 2*M->nnz; if ( ctrl.CType == MATCH_POWERLAW_FC ) { int ii; nnzRefinedGraph = 0; for ( ii=0; ii<cgraph->finer->nvtxs; ii++ ) { int tx=cgraph->finer->cmap[ii]; nnzRefinedGraph += M->xadj[tx+1] - M->xadj[tx]; } } else if ( ctrl.CType == MATCH_HASH && levels <= 3 ) { int ii; nnzRefinedGraph=0; for ( ii=0; ii<cgraph->nvtxs; ii++ ) { nnzRefinedGraph += cgraph->vwgt[ii] * (M->xadj[ii+1] - M->xadj[ii]); } } Mnew=propagateFlow(M,cgraph,cgraph->finer,nnzRefinedGraph); //printf("times:%d, levels:%d Mnew\n",times_rmcl, levels); //printMatrix(Mnew); if ( M != NULL ) { freeMatrix(M); } M=Mnew; } cgraph=cgraph->finer; //change to the finer graph // These two didn't get freed earlier, when we freed // gdata. levels--; printf("Done level %d (%d-th times MLR-MCL)\n", levels+1,times_rmcl);fflush(stdout); } //update clusters idxcopy(nvtxs, M->attractors, thisIndices); getAttractorsForAll(M, thisAttractor); for(int i=0; i<M->nvtxs; i++){ if(thisAttractor[i]){ countAttractor[0][i]++; #ifdef TEST_OUTPUT printf("attractor vid:%d(%d times), %d-th times MLRMCL\n",i+1,countAttractor[0][i],times_rmcl); #endif int level = 1, nodeIdx = i; for(cgraph = cgraphFinest; cgraph->coarser!=NULL; cgraph = cgraph->coarser, level++){ nodeIdx = cgraph->cmap[nodeIdx]; countAttractor[level][nodeIdx]++; } //printf("attractor:%d, times:%d\n",i+1,times_rmcl); } } //construct hash table for(int i=0;i<nvtxs;i++) newHashtable[i]=-1; // clear newHashtable. int new_num_clusters = 0 ; for(int i=0;i<nvtxs;i++){ if ( newHashtable[thisIndices[i]] == -1){ newHashtable[thisIndices[i]] = num_clusters+new_num_clusters; //cluster ID if(num_clusters+new_num_clusters == 22704 || num_clusters+new_num_clusters == 29925) printf("cluster #%d's attractor node %d\n",num_clusters+new_num_clusters,thisIndices[i]); new_num_clusters++; } } //construct clusters for(int i=0; i<new_num_clusters; i++){ std::set<idxtype>* newCluster = new std::set<idxtype>(); clusters.push_back(*newCluster); } for(int i=0; i<nvtxs ; i++){ if( thisIndices[i] != -1){ idxtype cID = newHashtable[thisIndices[i]]; indices[i].insert(cID); if(cID == -1) continue; clusters[ cID ].insert( i ); } } printf("number of clusters in %dth times MLR-MCL: %d\ttotal # clusters:%zu\n", times_rmcl, new_num_clusters,clusters.size()); num_clusters += new_num_clusters; //detect last attractor is split to multiple clusters /* double* countToCluster = (double*) malloc(sizeof(double) * num_clusters) ; for(idxtype vID=0; vID<nvtxs ; vID++){ //printf("test:%d\n",vID+1);fflush(stdout); if(lastAttractor[vID] && !thisAttractor[vID]){ for(int i=0; i<num_clusters; i++) countToCluster[i] = 0; int lastCluster = oldHashtable[lastIndices[vID]]; //printf("lastCluster:cid%d (attractor:%d size:%zu), num clusters:%d\n",lastCluster,vID+1,clusters[lastCluster].size(),num_clusters);fflush(stdout); for(std::set<idxtype>::iterator nodeIterator = clusters[lastCluster].begin(); nodeIterator != clusters[lastCluster].end(); nodeIterator++){ int currentCluster = newHashtable[thisIndices[*nodeIterator]]; countToCluster[currentCluster] ++; } for(idxtype cID = 0; cID < num_clusters; cID++){ if(countToCluster[cID] / clusters[lastCluster].size() >= opt.ratio_nodes_another_cluster && clusters[cID].find(vID)==clusters[cID].end() ){ //clusters[ cID ].insert(vID); //indices[ vID ].insert(cID); #ifdef TEST_OUTPUT printf("^add extra cluster: vid:%d to cid:%d\n",vID+1, cID);fflush(stdout); #endif } } } } free(countToCluster);*/ lastIndices = thisIndices; lastAttractor = thisAttractor; oldHashtable = newHashtable; } printf("total number of clusters after %d times MLR-MCL:%zu\n", opt.time_rmcl, clusters.size());fflush(stdout); free(hashtable); for(int i=0; i <= num_level ; i++) free(countAttractor[i]); free(countAttractor); free(isAttractor); free(firstIndices); free(thisIndices); free(newHashtable); free(thisAttractor); freeMatrix(M); return clusters; }
int main() { // LOAD DATA arma::mat X; // A) Toy Data // char inputFile[] = "../data_files/toyclusters/toyclusters.dat"; // B) X4.dat //char inputFile[] = "./X4.dat"; // C) fisher data //char inputFile[] = "./fisher.dat"; // D) MNIST data //char inputFile[] = "../data_files/MNIST/MNIST.dat"; // E) Reduced MNIST (5000x400) //char inputFile[] = "../data_files/MNIST/MNISTreduced.dat"; // F) Reduced MNIST (0 and 1) (1000x400) //char inputFile[] = "../data_files/MNIST/MNISTlittle.dat"; // G) Girl.png (512x768, RGB, already unrolled) //char inputFile[] = "girl.dat"; // H) Pool.png (383x512, RGB, already unrolled) //char inputFile[] = "pool.dat"; // I) Cat.png (733x490, RGB, unrolled) //char inputFile[] = "cat.dat"; // J) Airplane.png (512x512, RGB, unrolled) //char inputFile[] = "airplane.dat"; // K) Monarch.png (512x768, RGB, unrolled) //char inputFile[] = "monarch.dat"; // L) tulips.png (512x768 ,RGB, unrolled) //char inputFile[] = "tulips.dat"; // M) demo.dat (2d data) char inputFile[] = "demo.dat"; // INITIALIZE PARAMETERS X.load(inputFile); const arma::uword N = X.n_rows; const arma::uword D = X.n_cols; arma::umat ids(N,1); // needed to shuffle indices later arma::umat shuffled_ids(N,1); for (arma::uword i = 0; i < N; ++i) { ids(i,0) = i; } arma::arma_rng::set_seed_random(); // set arma rng // int seed = time(NULL); // set RNG seed to current time // srand(seed); arma::uword initial_K = 32; // initial number of clusters arma::uword K = initial_K; arma::umat clusters(N,1); // contains cluster assignments for each data point for (arma::uword i = 0; i < N; ++i) { clusters(i, 0) = i%K; // initialize as [0,1,...,K-1,0,1,...,K-1,0,...] } arma::umat cluster_sizes(N,1,arma::fill::zeros); // contains num data points in cluster k for (arma::uword i = 0; i < N; ++i) { cluster_sizes(clusters(i,0), 0) += 1; } arma::mat mu(N, D, arma::fill::zeros); // contains cluster mean parameters arma::mat filler(D,D,arma::fill::eye); std::vector<arma::mat> sigma(N,filler); // contains cluster covariance parameters if (K < N) { // set parameters not belonging to any cluster to -999 mu.rows(K,N-1).fill(-999); for (arma::uword k = K; k < N; ++k) { sigma[k].fill(-999); } } arma::umat uword_dummy(1,1); // dummy 1x1 matrix; // for (arma::uword i = 0; i <N; ++i) { // std::cout << sigma[i] << std::endl; // } // std::cout << X << std::endl // << N << std::endl // << D << std::endl // << K << std::endl // << clusters << std::endl // << cluster_sizes << std::endl // << ids << std::endl; // INITIALIZE HYPER PARAMETERS // Dirichlet Process concentration parameter is alpha: double alpha = 1; // Dirichlet Process base distribution (i.e. prior) is // H(mu,sigma) = NIW(mu,Sigma|m_0,k_0,S_0,nu_0) = N(mu|m_0,Sigma/k_0)IW(Sigma|S_0,nu_0) arma::mat perturbation(D,D,arma::fill::eye); perturbation *= 0.000001; //const arma::mat S_0 = arma::cov(X,X,1) + perturbation; // S_xbar / N const arma::mat S_0(D,D,arma::fill::eye); const double nu_0 = D + 2; const arma::mat m_0 = mean(X).t(); const double k_0 = 0.01; // std::cout << "S_0" << S_0 << std::endl; // std::cout << S_0 << std::endl // << nu_0 << std::endl // << m_0 << std::endl // << k_0 << std::endl; // INITIALIZE SAMPLING PARAMETERS arma::uword NUM_SWEEPS = 250; // number of Gibbs iterations bool SAVE_CHAIN = false; // save output of each Gibbs iteration? // arma::uword BURN_IN = NUM_SWEEPS - 10; // arma::uword CHAINSIZE = NUM_SWEEPS - BURN_IN; // std::vector<arma::uword> chain_K(CHAINSIZE, K); // Initialize chain variable to initial parameters for convinience // std::vector<arma::umat> chain_clusters(CHAINSIZE, clusters); // std::vector<arma::umat> chain_clusterSizes(CHAINSIZE, cluster_sizes); // std::vector<arma::mat> chain_mu(CHAINSIZE, mu); // std::vector<std::vector<arma::mat> > chain_sigma(CHAINSIZE, sigma); // for (arma::uword sweep = 0; sweep < CHAINSIZE; ++sweep) { // std::cout << sweep << " K\n" << chain_K[sweep] << std::endl // << sweep << " clusters\n" << chain_clusters[sweep] << std::endl // << sweep << " sizes\n" << chain_clusterSizes[sweep] << std::endl // << sweep << " mu\n" << chain_mu[sweep] << std::endl; // for (arma::uword i = 0; i < N; ++i) { // std::cout << sweep << " " << i << " sigma\n" << chain_sigma[sweep][i] << std::endl; // } // } // START CHAIN std::cout << "Starting Algorithm with K = " << K << std::endl; for (arma::uword sweep = 0; sweep < NUM_SWEEPS; ++sweep) { // shuffle indices shuffled_ids = shuffle(ids); // std::cout << shuffled_ids << std::endl; // SAMPLE CLUSTERS for (arma::uword j = 0; j < N; ++j){ // std::cout << "j = " << j << std::endl; arma::uword i = shuffled_ids(j); arma::mat x = X.row(i).t(); // current data point // Remove i's statistics and any empty clusters arma::uword c = clusters(i,0); // current cluster cluster_sizes(c,0) -= 1; //std::cout << "old c = " << c << std::endl; if (cluster_sizes(c,0) == 0) { // remove empty cluster cluster_sizes(c,0) = cluster_sizes(K-1,0); // move entries for K onto position c mu.row(c) = mu.row(K-1); sigma[c] = sigma[K-1]; uword_dummy(0,0) = c; arma::uvec idx = find(clusters == K - 1); clusters.each_row(idx) = uword_dummy; cluster_sizes(K-1,0) = 0; mu.row(K-1).fill(-999); sigma[K-1].fill(-999); --K; } // quick test of logMvnPdf: // arma::mat m_(2,1); // arma::mat s_(2,2); // arma::mat t_(2,1); // m_ << 1 << arma::endr << 2; // s_ << 3 << -0.2 << arma::endr << -0.2 << 1; // t_ << -3 << arma::endr << -3; // double lpdf = logMvnPdf(t_, m_, s_); // std::cout << lpdf << std::endl; // śhould be -19.1034 (works) // Find categorical distribution over clusters (tested) arma::mat logP(K+1, 1, arma::fill::zeros); // quick test of logInvWishPdf // arma::mat si_(2,2), s_(2,2); // double nu_ = 4; // si_ << 1 << 0.5 << arma::endr << 0.5 << 4; // s_ << 3 << -0.2 << arma::endr << -0.2 << 1; // double lpdf = logInvWishPdf(si_, s_, nu_); // std::cout << lpdf << std::endl; // should be -7.4399 (it is) // quick test for logNormInvWishPdf // arma::mat si_(2,2), s_(2,2); // arma :: mat mu_(2,1), m_(2,1); // double k_ = 0.5, nu_ = 4; // si_ << 1 << 0.5 << arma::endr << 0.5 << 4; // s_ << 3 << -0.2 << arma::endr << -0.2 << 1; // mu_ << -3 << arma::endr << -3; // m_ << 1 << arma::endr << 2; // double lp = logNormInvWishPdf(mu_,si_,m_,k_,s_,nu_); // std::cout << lp << std::endl; // should equal -15.2318 (it is) // p(existing clusters) (tested) for (arma::uword k = 0; k < K; ++k) { arma::mat m_ = mu.row(k).t(); arma::mat s_ = sigma[k]; logP(k,0) = log(cluster_sizes(k,0)) - log(N-1+alpha) + logMvnPdf(x,m_,s_); } // p(new cluster): find partition function (tested) // arma::mat dummy_mu(D, 1, arma::fill::zeros); // arma::mat dummy_sigma(D, D, arma::fill::eye); // double logPrior, logLklihd, logPstr, logPartition; // posterior hyperparameters (tested) arma::mat m_1(D,1), S_1(D,D); double k_1, nu_1; k_1 = k_0 + 1; nu_1 = nu_0 + 1; m_1 = (k_0*m_0 + x) / k_1; S_1 = S_0 + x * x.t() + k_0 * (m_0 * m_0.t()) - k_1 * (m_1 * m_1.t()); // std::cout << k_1 << std::endl // << nu_1 << std::endl // << m_1 << std::endl // << S_1 << std::endl; // // partition = likelihood*prior/posterior (tested) // // (perhaps a direct computation of the partition function would be better) // logPrior = logNormInvWishPdf(dummy_mu, dummy_sigma, m_0, k_0, S_0, nu_0); // logLklihd = logMvnPdf(x, dummy_mu, dummy_sigma); // logPstr = logNormInvWishPdf(dummy_mu, dummy_sigma, m_1, k_1, S_1, nu_1); // logPartition = logPrior + logLklihd - logPstr; // std::cout << "log Prior = " << logPrior << std::endl // << "log Likelihood = " << logLklihd << std::endl // << "log Posterior = " << logPstr << std::endl // << "log Partition = " << logPartition << std::endl; // Computing partition directly double logS0,signS0,logS1,signS1; arma::log_det(logS0,signS0,S_0); arma::log_det(logS1,signS1,S_1); /*std::cout << "log(det(S_0)) = " << logS0 << std::endl << "log(det(S_1)) = " << logS1 << std::endl;*/ double term1 = 0.5*D*(log(k_0)-log(k_1)); double term2 = -0.5*D*log(arma::datum::pi); double term3 = 0.5*(nu_0*logS0 - nu_1*logS1); double term4 = lgamma(0.5*nu_1); double term5 = -lgamma(0.5*(nu_1-D)); double logPartition = term1+term2+term3+term4+term5; /*double logPartition = 0.5*D*(log(k_0)-log(k_1)-log(arma::datum::pi)) \ /+0.5*(nu_0*logS0 - nu_1*logS1) + lgamma(0.5*nu_1) - lgamma(0.5*(nu_1-D));*/ /* std::cout << "term1 = " << term1 << std::endl << "term2 = " << term2 << std::endl << "term3 = " << term3 << std::endl << "term4 = " << term4 << std::endl << "term5 = " << term5 << std::endl;*/ //std::cout << "logP = " << logPartition << std::endl; // p(new cluster): (tested) logP(K,0) = log(alpha) - log(N - 1 + alpha) + logPartition; // std::cout << "logP(new cluster) = " << logP(K,0) << std::endl; //if(i == 49) //assert(false); // sample cluster for i arma::uword c_ = logCatRnd(logP); clusters(i,0) = c_; //if (j % 10 == 0){ //std::cout << "New c = " << c_ << std::endl; //std::cout << "logP = \n" << logP << std::endl; //} // quick test for mvnRnd // arma::mat mu, si; // mu << 1 << arma::endr << 2; // si << 1 << 0.9 << arma::endr << 0.9 << 1; // arma::mat m = mvnRnd(mu, si); // std::cout << m << std::endl; // quick test for invWishRnd // double df = 4; // arma::mat si(2,2); // si << 1 << 1 << arma::endr << 1 << 1; // arma::mat S = invWishRnd(si,df); // std::cout << S << std::endl; if (c_ == K) { // Sample parameters for any new-born clusters from posterior cluster_sizes(K, 0) = 1; arma::mat si_ = invWishRnd(S_1, nu_1); //arma::mat si_ = S_1; arma::mat mu_ = mvnRnd(m_1, si_/k_1); //arma::mat mu_ = m_1; mu.row(K) = mu_.t(); sigma[K] = si_; K += 1; } else { cluster_sizes(c_,0) += 1; } // if (sweep == 0) // std::cout << " K = " << K << std::endl; // // if (j == N-1) { // // std::cout << logP << std::endl; // // std::cout << K << std::endl; // // assert(false); // // } // std::cout << "K = " << K << "\n" << std::endl; } // sample CLUSTER PARAMETERS FROM POSTERIOR for (arma::uword k = 0; k < K; ++k) { // std::cout << "k = " << k << std::endl; // cluster data arma::mat Xk = X.rows(find(clusters == k)); arma::uword Nk = cluster_sizes(k,0); // posterior hyperparameters arma::mat m_Nk(D,1), S_Nk(D,D); double k_Nk, nu_Nk; arma::mat sum_k = sum(Xk,0).t(); arma::mat cov_k(D, D, arma::fill::zeros); for (arma::uword l = 0; l < Nk; ++l) { cov_k += Xk.row(l).t() * Xk.row(l); } k_Nk = k_0 + Nk; nu_Nk = nu_0 + Nk; m_Nk = (k_0 * m_0 + sum_k) / k_Nk; S_Nk = S_0 + cov_k + k_0 * (m_0 * m_0.t()) - k_Nk * (m_Nk * m_Nk.t()); // sample fresh parameters arma::mat si_ = invWishRnd(S_Nk, nu_Nk); //arma::mat si_ = S_Nk; arma::mat mu_ = mvnRnd(m_Nk, si_/k_Nk); //arma::mat mu_ = m_Nk; mu.row(k) = mu_.t(); sigma[k] = si_; } std::cout << "Iteration " << sweep + 1 << "/" << NUM_SWEEPS<< " done. K = " << K << std::endl; // // STORE CHAIN // if (SAVE_CHAIN) { // if (sweep >= BURN_IN) { // chain_K[sweep - BURN_IN] = K; // chain_clusters[sweep - BURN_IN] = clusters; // chain_clusterSizes[sweep - BURN_IN] = cluster_sizes; // chain_mu[sweep - BURN_IN] = mu; // chain_sigma[sweep - BURN_IN] = sigma; // } // } } std::cout << "Final cluster sizes: " << std::endl << cluster_sizes.rows(0, K-1) << std::endl; // WRITE OUPUT DATA TO FILE arma::mat MU = mu.rows(0,K-1); arma::mat SIGMA(D*K,D); for (arma::uword k = 0; k < K; ++k) { SIGMA.rows(k*D,(k+1)*D-1) = sigma[k]; } arma::umat IDX = clusters; // A) toycluster data // char MuFile[] = "../data_files/toyclusters/dpmMU.out"; // char SigmaFile[] = "../data_files/toyclusters/dpmSIGMA.out"; // char IdxFile[] = "../data_files/toyclusters/dpmIDX.out"; // B) X4.dat char MuFile[] = "dpmMU.out"; char SigmaFile[] = "dpmSIGMA.out"; char IdxFile[] = "dpmIDX.out"; std::ofstream KFile("K.out"); MU.save(MuFile, arma::raw_ascii); SIGMA.save(SigmaFile, arma::raw_ascii); IDX.save(IdxFile, arma::raw_ascii); KFile << "K = " << K << std::endl; if (SAVE_CHAIN) {} // std::ofstream chainKFile("chainK.out"); // std::ofstream chainClustersFile("chainClusters.out"); // std::ofstream chainClusterSizesFile("chainClusterSizes.out"); // std::ofstream chainMuFile("chainMu.out"); // std::ofstream chainSigmaFile("chainSigma.out"); // chainKFile << "Dirichlet Process Mixture Model.\nInput: " << inputFile << std::endl // << "Number of iterations of Gibbs Sampler: " << NUM_SWEEPS << std::endl // << "Burn-In: " << BURN_IN << std::endl // << "Initial number of clusters: " << initial_K << std::endl // << "Output: Number of cluster (K)\n" << std::endl; // chainClustersFile << "Dirichlet Process Mixture Model.\nInput: " << inputFile << std::endl // << "Number of iterations of Gibbs Sampler: " << NUM_SWEEPS << std::endl // << "Burn-In: " << BURN_IN << std::endl // << "Initial number of clusters: " << initial_K << std::endl // << "Output: Cluster identities (clusters)\n" << std::endl; // chainClusterSizesFile << "Dirichlet Process Mixture Model.\nInput: " << inputFile << std::endl // << "Number of iterations of Gibbs Sampler: " << NUM_SWEEPS << std::endl // << "Burn-In: " << BURN_IN << std::endl // << "Initial number of clusters: " << initial_K << std::endl // << "Output: Size of clusters (cluster_sizes)\n" << std::endl; // chainMuFile << "Dirichlet Process Mixture Model.\nInput: " << inputFile << std::endl // << "Number of iterations of Gibbs Sampler: " << NUM_SWEEPS << std::endl // << "Burn-In: " << BURN_IN << std::endl // << "Initial number of clusters: " << initial_K << std::endl // << "Output: Samples for cluster mean parameters (mu. Note: means stored in rows)\n" << std::endl; // chainSigmaFile << "Dirichlet Process Mixture Model.\nInput: " << inputFile << std::endl // << "Number of iterations of Gibbs Sampler: " << NUM_SWEEPS << std::endl // << "Burn-In " << BURN_IN << std::endl // << "Initial number of clusters: " << initial_K << std::endl // << "Output: Samples for cluster covariances (sigma)\n" << std::endl; // for (arma::uword sweep = 0; sweep < CHAINSIZE; ++sweep) { // arma::uword K = chain_K[sweep]; // chainKFile << "Sweep #" << BURN_IN + sweep + 1 << "\n" << chain_K[sweep] << std::endl; // chainClustersFile << "Sweep #" << BURN_IN + sweep + 1 << "\n" << chain_clusters[sweep] << std::endl; // chainClusterSizesFile << "Sweep #" << BURN_IN + sweep + 1 << "\n" << chain_clusterSizes[sweep].rows(0, K - 1) << std::endl; // chainMuFile << "Sweep #" << BURN_IN + sweep + 1 << "\n" << chain_mu[sweep].rows(0, K - 1) << std::endl; // chainSigmaFile << "Sweep #" << BURN_IN + sweep + 1<< "\n"; // for (arma::uword i = 0; i < K; ++i) { // chainSigmaFile << chain_sigma[sweep][i] << std::endl; // } // chainSigmaFile << std::endl; // } // } return 0; }
int main (int argc, char** argv) { // Data containers used pcl::PointCloud<PointTypeIO>::Ptr cloud_in (new pcl::PointCloud<PointTypeIO>), cloud_out (new pcl::PointCloud<PointTypeIO>); pcl::PointCloud<PointTypeFull>::Ptr cloud_with_normals (new pcl::PointCloud<PointTypeFull>); pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters), small_clusters (new pcl::IndicesClusters), large_clusters (new pcl::IndicesClusters); pcl::search::KdTree<PointTypeIO>::Ptr search_tree (new pcl::search::KdTree<PointTypeIO>); pcl::console::TicToc tt; // Load the input point cloud std::cerr << "Loading...\n", tt.tic (); pcl::io::loadPCDFile (argv[1], *cloud_in); std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_in->points.size () << " points\n"; // Downsample the cloud using a Voxel Grid class std::cerr << "Downsampling...\n", tt.tic (); pcl::VoxelGrid<PointTypeIO> vg; vg.setInputCloud (cloud_in); vg.setLeafSize (80.0, 80.0, 80.0); vg.setDownsampleAllData (true); vg.filter (*cloud_out); std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_out->points.size () << " points\n"; // Set up a Normal Estimation class and merge data in cloud_with_normals std::cerr << "Computing normals...\n", tt.tic (); pcl::copyPointCloud (*cloud_out, *cloud_with_normals); pcl::NormalEstimation<PointTypeIO, PointTypeFull> ne; ne.setInputCloud (cloud_out); ne.setSearchMethod (search_tree); ne.setRadiusSearch (300.0); ne.compute (*cloud_with_normals); std::cerr << ">> Done: " << tt.toc () << " ms\n"; // Set up a Conditional Euclidean Clustering class std::cerr << "Segmenting to clusters...\n", tt.tic (); pcl::ConditionalEuclideanClustering<PointTypeFull> cec (true); cec.setInputCloud (cloud_with_normals); cec.setConditionFunction (&customRegionGrowing); cec.setClusterTolerance (500.0); cec.setMinClusterSize (cloud_with_normals->points.size () / 1000); cec.setMaxClusterSize (cloud_with_normals->points.size () / 5); cec.segment (*clusters); cec.getRemovedClusters (small_clusters, large_clusters); std::cerr << ">> Done: " << tt.toc () << " ms\n"; // Using the intensity channel for lazy visualization of the output for (int i = 0; i < small_clusters->size (); ++i) for (int j = 0; j < (*small_clusters)[i].indices.size (); ++j) cloud_out->points[(*small_clusters)[i].indices[j]].intensity = -2.0; for (int i = 0; i < large_clusters->size (); ++i) for (int j = 0; j < (*large_clusters)[i].indices.size (); ++j) cloud_out->points[(*large_clusters)[i].indices[j]].intensity = +10.0; for (int i = 0; i < clusters->size (); ++i) { int label = rand () % 8; for (int j = 0; j < (*clusters)[i].indices.size (); ++j) cloud_out->points[(*clusters)[i].indices[j]].intensity = label; } // Save the output point cloud std::cerr << "Saving...\n", tt.tic (); pcl::io::savePCDFile ("output.pcd", *cloud_out); std::cerr << ">> Done: " << tt.toc () << " ms\n"; return (0); }
void JLinkage::clusterPSMatrix(std::vector<std::vector<int> >& result) { /* allocate memory */ std::vector<std::vector<double> > distances(line_count, std::vector<double>(line_count, 0)); std::vector<std::vector<int> > clusters(line_count, std::vector<int>(line_count, 0)); std::vector<int> indicators(line_count, 1); /* initialization */ double minDis = 1; int indexA = 0, indexB = 0; for(int i = 0; i < line_count; i++) { clusters[i][i] = 1; for(int j = i + 1; j < line_count; j++) { const double jd = jaccardDist(PSMatrix[i], PSMatrix[j]); distances[i][j] = jd; distances[j][i] = jd; if(jd < minDis) { minDis = jd; indexA = i; indexB = j; } } } while(minDis != 1) { /* merge two clusters */ for(int i = 0; i < line_count; i++) { if(clusters[indexA][i] || clusters[indexB][i]) clusters[indexA][i] = clusters[indexB][i] = 1; } indicators[indexB] = 0; for(int i = 0; i < JLINKAGE_MODEL_SIZE; i++) { PSMatrix[indexA] = PSMatrix[indexB] = PSMatrix[indexA]&PSMatrix[indexB]; } /* recalculate distance */ for(int i = 0; i < line_count; i++) { distances[indexA][i] = jaccardDist(PSMatrix[indexA], PSMatrix[i]); distances[i][indexA] = distances[indexA][i]; } /* find minimum distance */ minDis = 1; for(int i = 0; i < line_count; i++) { if(indicators[i] == 0) continue; for(int j = i + 1; j < line_count; j++) { if(indicators[j] == 0) continue; if(distances[i][j] < minDis) { minDis = distances[i][j]; indexA = i; indexB = j; } } } } /* calculate cluster size */ std::vector<int> clusterSizes(line_count); for(int i = 0; i < line_count; i++) { int cs = 0; if(indicators[i]) { for(int j = 0; j < line_count; j++) { if(clusters[i][j]) ++cs; } } clusterSizes[i] = cs; } const int cluster_num = 3; result.clear(); result.resize(cluster_num, std::vector<int>(line_count)); /* choose the largest three clusters */ int count = 0; while(count < cluster_num) { int max_index = 0; int max_size = clusterSizes[0]; for(int i = 1; i < line_count; i++) { if(max_size < clusterSizes[i]) { max_size = clusterSizes[i]; max_index = i; } } result[count] = clusters[max_index]; count++; clusterSizes[max_index] = 0; } /* print clusters */ /* for(int i = 0; i < cluster_num; i++) { printf("Cluster %d:\n", i); for(int j = 0; j < line_count; j++) { if(result[i][j]) printf("%d ", j); } printf("\n"); } */ }
void AverageLinkage::operator()(DistanceMatrix<float> & original_distance, std::vector<BinaryTreeNode> & cluster_tree, const float threshold /*=1*/) const { // input MUST have >= 2 elements! if (original_distance.dimensionsize() < 2) { throw ClusterFunctor::InsufficientInput(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "Distance matrix to start from only contains one element"); } std::vector<std::set<Size> > clusters(original_distance.dimensionsize()); for (Size i = 0; i < original_distance.dimensionsize(); ++i) { clusters[i].insert(i); } cluster_tree.clear(); cluster_tree.reserve(original_distance.dimensionsize() - 1); // Initial minimum-distance pair original_distance.updateMinElement(); std::pair<Size, Size> min = original_distance.getMinElementCoordinates(); Size overall_cluster_steps(original_distance.dimensionsize()); startProgress(0, original_distance.dimensionsize(), "clustering data"); while (original_distance(min.second, min.first) < threshold) { //grow the tree cluster_tree.push_back(BinaryTreeNode(*(clusters[min.second].begin()), *(clusters[min.first].begin()), original_distance(min.first, min.second))); if (cluster_tree.back().left_child > cluster_tree.back().right_child) { std::swap(cluster_tree.back().left_child, cluster_tree.back().right_child); } if (original_distance.dimensionsize() > 2) { //pick minimum-distance pair i,j and merge them //calculate parameter for lance-williams formula float alpha_i = (float)(clusters[min.first].size() / (float)(clusters[min.first].size() + clusters[min.second].size())); float alpha_j = (float)(clusters[min.second].size() / (float)(clusters[min.first].size() + clusters[min.second].size())); //~ std::cout << alpha_i << '\t' << alpha_j << std::endl; //pushback elements of second to first (and then erase second) clusters[min.second].insert(clusters[min.first].begin(), clusters[min.first].end()); // erase first one clusters.erase(clusters.begin() + min.first); //update original_distance matrix //average linkage: new distance between clusters is the minimum distance between elements of each cluster //lance-williams update for d((i,j),k): (m_i/m_i+m_j)* d(i,k) + (m_j/m_i+m_j)* d(j,k) ; m_x is the number of elements in cluster x for (Size k = 0; k < min.second; ++k) { float dik = original_distance.getValue(min.first, k); float djk = original_distance.getValue(min.second, k); original_distance.setValueQuick(min.second, k, (alpha_i * dik + alpha_j * djk)); } for (Size k = min.second + 1; k < original_distance.dimensionsize(); ++k) { float dik = original_distance.getValue(min.first, k); float djk = original_distance.getValue(min.second, k); original_distance.setValueQuick(k, min.second, (alpha_i * dik + alpha_j * djk)); } //reduce original_distance.reduce(min.first); //update minimum-distance pair original_distance.updateMinElement(); //get min-pair from triangular matrix min = original_distance.getMinElementCoordinates(); } else { break; } setProgress(overall_cluster_steps - original_distance.dimensionsize()); //repeat until only two cluster remains, last step skips matrix operations } //fill tree with dummy nodes Size sad(*clusters.front().begin()); for (Size i = 1; (i < clusters.size()) && (cluster_tree.size() < cluster_tree.capacity()); ++i) { cluster_tree.push_back(BinaryTreeNode(sad, *clusters[i].begin(), -1.0)); } endProgress(); }
bool MinDist::loadModelFromFile(fstream &file){ trained = false; numInputDimensions = 0; numClasses = 0; models.clear(); classLabels.clear(); if(!file.is_open()) { errorLog << "loadModelFromFile(string filename) - Could not open file to load model" << endl; return false; } std::string word; file >> word; //Check to see if we should load a legacy file if( word == "GRT_MINDIST_MODEL_FILE_V1.0" ){ return loadLegacyModelFromFile( file ); } //Find the file type header if(word != "GRT_MINDIST_MODEL_FILE_V2.0"){ errorLog << "loadModelFromFile(string filename) - Could not find Model File Header" << endl; return false; } //Load the base settings from the file if( !Classifier::loadBaseSettingsFromFile(file) ){ errorLog << "loadModelFromFile(string filename) - Failed to load base settings from file!" << endl; return false; } if( trained ){ //Resize the buffer models.resize(numClasses); classLabels.resize(numClasses); //Load each of the K models for(UINT k=0; k<numClasses; k++){ double rejectionThreshold; double gamma; double trainingSigma; double trainingMu; file >> word; if( word != "ClassLabel:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the class label for class " << k << endl; return false; } file >> classLabels[k]; file >> word; if( word != "NumClusters:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the NumClusters for class " << k << endl; return false; } file >> numClusters; file >> word; if( word != "RejectionThreshold:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the RejectionThreshold for class " << k << endl; return false; } file >> rejectionThreshold; file >> word; if( word != "Gamma:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the Gamma for class " << k << endl; return false; } file >> gamma; file >> word; if( word != "TrainingMu:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the TrainingMu for class " << k << endl; return false; } file >> trainingMu; file >> word; if( word != "TrainingSigma:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the TrainingSigma for class " << k << endl; return false; } file >> trainingSigma; file >> word; if( word != "ClusterData:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the ClusterData for class " << k << endl; return false; } //Load the cluster data MatrixDouble clusters(numClusters,numInputDimensions); for(UINT i=0; i<numClusters; i++){ for(UINT j=0; j<numInputDimensions; j++){ file >> clusters[i][j]; } } models[k].setClassLabel( classLabels[k] ); models[k].setClusters( clusters ); models[k].setGamma( gamma ); models[k].setRejectionThreshold( rejectionThreshold ); models[k].setTrainingSigma( trainingSigma ); models[k].setTrainingMu( trainingMu ); } //Recompute the null rejection thresholds recomputeNullRejectionThresholds(); //Resize the prediction results to make sure it is setup for realtime prediction maxLikelihood = DEFAULT_NULL_LIKELIHOOD_VALUE; bestDistance = DEFAULT_NULL_DISTANCE_VALUE; classLikelihoods.resize(numClasses,DEFAULT_NULL_LIKELIHOOD_VALUE); classDistances.resize(numClasses,DEFAULT_NULL_DISTANCE_VALUE); } return true; }
void SingleLinkage::operator()(DistanceMatrix<float> & original_distance, std::vector<BinaryTreeNode> & cluster_tree, const float threshold /*=1*/) const { // input MUST have >= 2 elements! if (original_distance.dimensionsize() < 2) { throw ClusterFunctor::InsufficientInput(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "Distance matrix to start from only contains one element"); } cluster_tree.clear(); if (threshold < 1) { LOG_ERROR << "You tried to use Single Linkage clustering with a threshold. This is currently not supported!" << std::endl; throw Exception::NotImplemented(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION); } //SLINK std::vector<Size> pi; pi.reserve(original_distance.dimensionsize()); std::vector<float> lambda; lambda.reserve(original_distance.dimensionsize()); startProgress(0, original_distance.dimensionsize(), "clustering data"); //initialize first pointer values pi.push_back(0); lambda.push_back(std::numeric_limits<float>::max()); for (Size k = 1; k < original_distance.dimensionsize(); ++k) { std::vector<float> row_k; row_k.reserve(k); //initialize pointer values for element to cluster pi.push_back(k); lambda.push_back(std::numeric_limits<float>::max()); // get the right distances for (Size i = 0; i < k; ++i) { row_k.push_back(original_distance.getValue(i, k)); } //calculate pointer values for element k for (Size i = 0; i < k; ++i) { if (lambda[i] >= row_k[i]) { row_k[pi[i]] = std::min(row_k[pi[i]], lambda[i]); lambda[i] = row_k[i]; pi[i] = k; } else { row_k[pi[i]] = std::min(row_k[pi[i]], row_k[i]); } } //update clustering if necessary for (Size i = 0; i < k; ++i) { if (lambda[i] >= lambda[pi[i]]) { pi[i] = k; } } setProgress(k); } for (Size i = 0; i < pi.size() - 1; ++i) { //strict order is always kept in algorithm: i < pi[i] cluster_tree.push_back(BinaryTreeNode(i, pi[i], lambda[i])); //~ std::cout << i << '\n' << pi[i] << '\n' << lambda[i] << std::endl; } //sort pre-tree std::sort(cluster_tree.begin(), cluster_tree.end(), compareBinaryTreeNode); // convert -pre-tree to correct format for (Size i = 0; i < cluster_tree.size(); ++i) { if (cluster_tree[i].right_child < cluster_tree[i].left_child) { std::swap(cluster_tree[i].left_child, cluster_tree[i].right_child); } for (Size k = i + 1; k < cluster_tree.size(); ++k) { if (cluster_tree[k].left_child == cluster_tree[i].right_child) { cluster_tree[k].left_child = cluster_tree[i].left_child; } else if (cluster_tree[k].left_child > cluster_tree[i].right_child) { --cluster_tree[k].left_child; } if (cluster_tree[k].right_child == cluster_tree[i].right_child) { cluster_tree[k].right_child = cluster_tree[i].left_child; } else if (cluster_tree[k].right_child > cluster_tree[i].right_child) { --cluster_tree[k].right_child; } } } //~ prepare to redo clustering to get all indices for binarytree in min index element representation std::vector<std::set<Size> > clusters(original_distance.dimensionsize()); for (Size i = 0; i < original_distance.dimensionsize(); ++i) { clusters[i].insert(i); } for (Size cluster_step = 0; cluster_step < cluster_tree.size(); ++cluster_step) { Size new_left_child = *(clusters[cluster_tree[cluster_step].left_child].begin()); Size new_right_child = *(clusters[cluster_tree[cluster_step].right_child].begin()); clusters[cluster_tree[cluster_step].left_child].insert(clusters[cluster_tree[cluster_step].right_child].begin(), clusters[cluster_tree[cluster_step].right_child].end()); clusters.erase(clusters.begin() + cluster_tree[cluster_step].right_child); std::swap(cluster_tree[cluster_step].left_child, new_left_child); std::swap(cluster_tree[cluster_step].right_child, new_right_child); if (cluster_tree[cluster_step].left_child > cluster_tree[cluster_step].right_child) { std::swap(cluster_tree[cluster_step].left_child, cluster_tree[cluster_step].right_child); } } endProgress(); }
void InvaderDetectOperation::operate( const std::vector<ImageBufferPtr>& inputList, const std::vector<double>&, ImageBufferPtr output ) { assert(inputList.size() == 2); min.clear(); max.clear(); CpuImage input = readBuffer(inputList[0]); Matrix<int> clusters( input.height(), input.width() ); for(unsigned int y = 0; y < input.height(); y++) for(unsigned int x = 0; x < input.width(); x++) { clusters(y,x) = -1; } int lastCluster = 0; for(unsigned int y = 0; y < input.height(); y++ ) for(unsigned int x = 0; x < input.width(); x++ ) { if( clusters(y,x).get() != -1 ) continue; if( ! input(y,x).get().r ){ clusters(y,x) = 0; continue; } clusters(y,x) = ++lastCluster; std::queue<Recursive> recurse; recurse.push(Recursive(y,x)); while( !recurse.empty() ) { Recursive recursive = recurse.front(); recurse.pop(); int ry = recursive.ry; int rx = recursive.rx; for( int line = -1; line <= 1; line++){ for( int column = -1; column <= 1; column++){ if ( clusters(ry,rx).neighbour(line,column).get() == -1 && input(ry,rx).neighbour(line,column).get().r != 0 ){ clusters(ry, rx).neighbour(line, column) = lastCluster; recurse.push( Recursive(ry+line, rx+column) ); } } } }; } CpuImage backImage = readBuffer(inputList[1]); for( unsigned int y = 0; y < input.height(); y++) for( unsigned int x = 0; x < input.width(); x++) { maximize( backImage(y,x) , clusters(y,x).get() ); minimize( backImage(y,x) , clusters(y,x).get() ); } { auto minI = min.begin(); auto maxI = max.begin(); for( ; minI != min.end(); ++minI,++maxI) { if( minI->first == 0 ) continue; int clusterWidth = maxI->second.first-minI->second.first; int clusterHeight = maxI->second.second-minI->second.second; int area = clusterHeight*clusterWidth; if( area < 500 ) continue; double ratio = double(clusterHeight)/double(clusterWidth); // std::cout << minI->first << ": (" << clusterWidth << "," << clusterHeight << "), ratio:" << ratio; // std::cout << ", area:" << area << std::endl; int color = ratio > 1.2 ? 3 : 2; Point line = minI->second; for( ; line.second <= maxI->second.second; ++line.second) { colorize( backImage(line.second,line.first), color ); } for( ; line.first <= maxI->second.first; ++line.first) { colorize( backImage(line.second,line.first), color ); } for( ; line.second >= minI->second.second; --line.second) { colorize( backImage(line.second,line.first), color ); } for( ; line.first >= minI->second.first; --line.first) { colorize( backImage(line.second,line.first), color ); } } } writeBuffer(backImage,output); }
int spheroidsToSTL(const string& out, const shared_ptr<DemField>& dem, Real tol, const string& solid, int mask, bool append, bool clipCell, bool merge){ if(tol==0 || isnan(tol)) throw std::runtime_error("tol must be non-zero."); #ifndef WOO_GTS if(merge) throw std::runtime_error("woo.triangulated.spheroidsToSTL: merge=True only possible in builds with the 'gts' feature."); #endif // first traversal to find reference radius auto particleOk=[&](const shared_ptr<Particle>&p){ return (mask==0 || (p->mask & mask)) && (p->shape->isA<Sphere>() || p->shape->isA<Ellipsoid>() || p->shape->isA<Capsule>()); }; int numTri=0; if(tol<0){ LOG_DEBUG("tolerance is negative, taken as relative to minimum radius."); Real minRad=Inf; for(const auto& p: *dem->particles){ if(particleOk(p)) minRad=min(minRad,p->shape->equivRadius()); } if(isinf(minRad) || isnan(minRad)) throw std::runtime_error("Minimum radius not found (relative tolerance specified); no matching particles?"); tol=-minRad*tol; LOG_DEBUG("Minimum radius "<<minRad<<"."); } LOG_DEBUG("Triangulation tolerance is "<<tol); std::ofstream stl(out,append?(std::ofstream::app|std::ofstream::binary):std::ofstream::binary); // binary better, anyway if(!stl.good()) throw std::runtime_error("Failed to open output file "+out+" for writing."); Scene* scene=dem->scene; if(!scene) throw std::logic_error("DEM field has not associated scene?"); // periodicity, cache that for later use AlignedBox3r cell; /* wasteful memory-wise, but we need to store the whole triangulation in case *merge* is in effect, when it is only an intermediary result and will not be output as-is */ vector<vector<Vector3r>> ppts; vector<vector<Vector3i>> ttri; vector<Particle::id_t> iid; for(const auto& p: *dem->particles){ if(!particleOk(p)) continue; const auto sphere=dynamic_cast<Sphere*>(p->shape.get()); const auto ellipsoid=dynamic_cast<Ellipsoid*>(p->shape.get()); const auto capsule=dynamic_cast<Capsule*>(p->shape.get()); vector<Vector3r> pts; vector<Vector3i> tri; if(sphere || ellipsoid){ Real r=sphere?sphere->radius:ellipsoid->semiAxes.minCoeff(); // 1 is for icosahedron int tess=ceil(M_PI/(5*acos(1-tol/r))); LOG_DEBUG("Tesselation level for #"<<p->id<<": "<<tess); tess=max(tess,0); auto uSphTri(CompUtils::unitSphereTri20(/*0 for icosahedron*/max(tess-1,0))); const auto& uPts=std::get<0>(uSphTri); // unit sphere point coords pts.resize(uPts.size()); const auto& node=(p->shape->nodes[0]); Vector3r scale=(sphere?sphere->radius*Vector3r::Ones():ellipsoid->semiAxes); for(size_t i=0; i<uPts.size(); i++){ pts[i]=node->loc2glob(uPts[i].cwiseProduct(scale)); } tri=std::get<1>(uSphTri); // this makes a copy, but we need out own for capsules } if(capsule){ #ifdef WOO_VTK int subdiv=max(4.,ceil(M_PI/(acos(1-tol/capsule->radius)))); std::tie(pts,tri)=VtkExport::triangulateCapsule(static_pointer_cast<Capsule>(p->shape),subdiv); #else throw std::runtime_error("Triangulation of capsules is (for internal and entirely fixable reasons) only available when compiled with the 'vtk' features."); #endif } // do not write out directly, store first for later ppts.push_back(pts); ttri.push_back(tri); LOG_TRACE("#"<<p->id<<" triangulated: "<<tri.size()<<","<<pts.size()<<" faces,vertices."); if(scene->isPeriodic){ // make sure we have aabb, in skewed coords and such if(!p->shape->bound){ // this is a bit ugly, but should do the trick; otherwise we would recompute all that ourselves here if(sphere) Bo1_Sphere_Aabb().go(p->shape); else if(ellipsoid) Bo1_Ellipsoid_Aabb().go(p->shape); else if(capsule) Bo1_Capsule_Aabb().go(p->shape); } assert(p->shape->bound); const AlignedBox3r& box(p->shape->bound->box); AlignedBox3r cell(Vector3r::Zero(),scene->cell->getSize()); // possibly in skewed coords // central offset Vector3i off0; scene->cell->canonicalizePt(p->shape->nodes[0]->pos,off0); // computes off0 Vector3i off; // offset from the original cell //cerr<<"#"<<p->id<<" at "<<p->shape->nodes[0]->pos.transpose()<<", off0="<<off0<<endl; for(off[0]=off0[0]-1; off[0]<=off0[0]+1; off[0]++) for(off[1]=off0[1]-1; off[1]<=off0[1]+1; off[1]++) for(off[2]=off0[2]-1; off[2]<=off0[2]+1; off[2]++){ Vector3r dx=scene->cell->intrShiftPos(off); //cerr<<" off="<<off.transpose()<<", dx="<<dx.transpose()<<endl; AlignedBox3r boxOff(box); boxOff.translate(dx); //cerr<<" boxOff="<<boxOff.min()<<";"<<boxOff.max()<<" | cell="<<cell.min()<<";"<<cell.max()<<endl; if(boxOff.intersection(cell).isEmpty()) continue; // copy the entire triangulation, offset by dx vector<Vector3r> pts2(pts); for(auto& p: pts2) p+=dx; vector<Vector3i> tri2(tri); // same topology ppts.push_back(pts2); ttri.push_back(tri2); LOG_TRACE(" offset "<<off.transpose()<<": #"<<p->id<<": "<<tri2.size()<<","<<pts2.size()<<" faces,vertices."); } } } if(!merge){ LOG_DEBUG("Will export (unmerged) "<<ppts.size()<<" particles to STL."); stl<<"solid "<<solid<<"\n"; for(size_t i=0; i<ppts.size(); i++){ const auto& pts(ppts[i]); const auto& tri(ttri[i]); LOG_TRACE("Exporting "<<i<<" with "<<tri.size()<<" faces."); for(const Vector3i& t: tri){ Vector3r pp[]={pts[t[0]],pts[t[1]],pts[t[2]]}; // skip triangles which are entirely out of the canonical periodic cell if(scene->isPeriodic && clipCell && (!scene->cell->isCanonical(pp[0]) && !scene->cell->isCanonical(pp[1]) && !scene->cell->isCanonical(pp[2]))) continue; numTri++; Vector3r n=(pp[1]-pp[0]).cross(pp[2]-pp[1]).normalized(); stl<<" facet normal "<<n.x()<<" "<<n.y()<<" "<<n.z()<<"\n"; stl<<" outer loop\n"; for(auto p: {pp[0],pp[1],pp[2]}){ stl<<" vertex "<<p[0]<<" "<<p[1]<<" "<<p[2]<<"\n"; } stl<<" endloop\n"; stl<<" endfacet\n"; } } stl<<"endsolid "<<solid<<"\n"; stl.close(); return numTri; } #if WOO_GTS /***** Convert all triangulation to GTS surfaces, find their distances, isolate connected components, merge these components incrementally and write to STL *****/ // total number of points const size_t N(ppts.size()); // bounds for collision detection struct Bound{ Bound(Real _coord, int _id, bool _isMin): coord(_coord), id(_id), isMin(_isMin){}; Bound(): coord(NaN), id(-1), isMin(false){}; // just for allocation Real coord; int id; bool isMin; bool operator<(const Bound& b) const { return coord<b.coord; } }; vector<Bound> bounds[3]={vector<Bound>(2*N),vector<Bound>(2*N),vector<Bound>(2*N)}; /* construct GTS surface objects; all objects must be deleted explicitly! */ vector<GtsSurface*> ssurf(N); vector<vector<GtsVertex*>> vvert(N); vector<vector<GtsEdge*>> eedge(N); vector<AlignedBox3r> boxes(N); for(size_t i=0; i<N; i++){ LOG_TRACE("** Creating GTS surface for #"<<i<<", with "<<ttri[i].size()<<" faces, "<<ppts[i].size()<<" vertices."); AlignedBox3r box; // new surface object ssurf[i]=gts_surface_new(gts_surface_class(),gts_face_class(),gts_edge_class(),gts_vertex_class()); // copy over all vertices vvert[i].reserve(ppts[i].size()); eedge[i].reserve(size_t(1.5*ttri[i].size())); // each triangle consumes 1.5 edges, for closed surfs for(size_t v=0; v<ppts[i].size(); v++){ vvert[i].push_back(gts_vertex_new(gts_vertex_class(),ppts[i][v][0],ppts[i][v][1],ppts[i][v][2])); box.extend(ppts[i][v]); } // create faces, and create edges on the fly as needed std::map<std::pair<int,int>,int> edgeIndices; for(size_t t=0; t<ttri[i].size(); t++){ //const Vector3i& t(ttri[i][t]); //LOG_TRACE("Face with vertices "<<ttri[i][t][0]<<","<<ttri[i][t][1]<<","<<ttri[i][t][2]); Vector3i eIxs; for(int a:{0,1,2}){ int A(ttri[i][t][a]), B(ttri[i][t][(a+1)%3]); auto AB=std::make_pair(min(A,B),max(A,B)); auto ABI=edgeIndices.find(AB); if(ABI==edgeIndices.end()){ // this edge not created yet edgeIndices[AB]=eedge[i].size(); // last index eIxs[a]=eedge[i].size(); //LOG_TRACE(" New edge #"<<eIxs[a]<<": "<<A<<"--"<<B<<" (length "<<(ppts[i][A]-ppts[i][B]).norm()<<")"); eedge[i].push_back(gts_edge_new(gts_edge_class(),vvert[i][A],vvert[i][B])); } else { eIxs[a]=ABI->second; //LOG_TRACE(" Found edge #"<<ABI->second<<" for "<<A<<"--"<<B); } } //LOG_TRACE(" New face: edges "<<eIxs[0]<<"--"<<eIxs[1]<<"--"<<eIxs[2]); GtsFace* face=gts_face_new(gts_face_class(),eedge[i][eIxs[0]],eedge[i][eIxs[1]],eedge[i][eIxs[2]]); gts_surface_add_face(ssurf[i],face); } // make sure the surface is OK if(!gts_surface_is_orientable(ssurf[i])) LOG_ERROR("Surface of #"+to_string(iid[i])+" is not orientable (expect troubles)."); if(!gts_surface_is_closed(ssurf[i])) LOG_ERROR("Surface of #"+to_string(iid[i])+" is not closed (expect troubles)."); assert(!gts_surface_is_self_intersecting(ssurf[i])); // copy bounds LOG_TRACE("Setting bounds of surf #"<<i); boxes[i]=box; for(int ax:{0,1,2}){ bounds[ax][2*i+0]=Bound(box.min()[ax],/*id*/i,/*isMin*/true); bounds[ax][2*i+1]=Bound(box.max()[ax],/*id*/i,/*isMin*/false); } } /* broad-phase collision detection between GTS surfaces only those will be probed with exact algorithms below and merged if needed */ for(int ax:{0,1,2}) std::sort(bounds[ax].begin(),bounds[ax].end()); vector<Bound>& bb(bounds[0]); // run the search along x-axis, does not matter really std::list<std::pair<int,int>> int0; // broad-phase intersections for(size_t i=0; i<2*N; i++){ if(!bb[i].isMin) continue; // only start with lower bound // go up to the upper bound, but handle overflow safely (no idea why it would happen here) as well for(size_t j=i+1; j<2*N && bb[j].id!=bb[i].id; j++){ if(bb[j].isMin) continue; // this is handled by symmetry #if EIGEN_VERSION_AT_LEAST(3,2,5) if(!boxes[bb[i].id].intersects(boxes[bb[j].id])) continue; // no intersection along all axes #else // old, less elegant if(boxes[bb[i].id].intersection(boxes[bb[j].id]).isEmpty()) continue; #endif int0.push_back(std::make_pair(min(bb[i].id,bb[j].id),max(bb[i].id,bb[j].id))); LOG_TRACE("Broad-phase collision "<<int0.back().first<<"+"<<int0.back().second); } } /* narrow-phase collision detection between GTS surface this must be done via gts_surface_inter_new, since gts_surface_distance always succeeds */ std::list<std::pair<int,int>> int1; for(const std::pair<int,int> ij: int0){ LOG_TRACE("Testing narrow-phase collision "<<ij.first<<"+"<<ij.second); #if 0 GtsRange gr1, gr2; gts_surface_distance(ssurf[ij.first],ssurf[ij.second],/*delta ??*/(gfloat).2,&gr1,&gr2); if(gr1.min>0 && gr2.min>0) continue; LOG_TRACE(" GTS reports collision "<<ij.first<<"+"<<ij.second<<" (min. distances "<<gr1.min<<", "<<gr2.min); #else GtsSurface *s1(ssurf[ij.first]), *s2(ssurf[ij.second]); GNode* t1=gts_bb_tree_surface(s1); GNode* t2=gts_bb_tree_surface(s2); GtsSurfaceInter* I=gts_surface_inter_new(gts_surface_inter_class(),s1,s2,t1,t2,/*is_open_1*/false,/*is_open_2*/false); GSList* l=gts_surface_intersection(s1,s2,t1,t2); // list of edges describing intersection int n1=g_slist_length(l); // extra check by looking at number of faces of the intersected surface #if 1 GtsSurface* s12=gts_surface_new(gts_surface_class(),gts_face_class(),gts_edge_class(),gts_vertex_class()); gts_surface_inter_boolean(I,s12,GTS_1_OUT_2); gts_surface_inter_boolean(I,s12,GTS_2_OUT_1); int n2=gts_surface_face_number(s12); gts_object_destroy(GTS_OBJECT(s12)); #endif gts_bb_tree_destroy(t1,TRUE); gts_bb_tree_destroy(t2,TRUE); gts_object_destroy(GTS_OBJECT(I)); g_slist_free(l); if(n1==0) continue; #if 1 if(n2==0){ LOG_ERROR("n1==0 but n2=="<<n2<<" (no narrow-phase collision)"); continue; } #endif LOG_TRACE(" GTS reports collision "<<ij.first<<"+"<<ij.second<<" ("<<n<<" edges describe the intersection)"); #endif int1.push_back(ij); } /* connected components on the graph: graph nodes are 0…(N-1), graph edges are in int1 see http://stackoverflow.com/a/37195784/761090 */ typedef boost::subgraph<boost::adjacency_list<boost::vecS,boost::vecS,boost::undirectedS,boost::property<boost::vertex_index_t,int>,boost::property<boost::edge_index_t,int>>> Graph; Graph graph(N); for(const auto& ij: int1) boost::add_edge(ij.first,ij.second,graph); vector<size_t> clusters(boost::num_vertices(graph)); size_t numClusters=boost::connected_components(graph,clusters.data()); for(size_t n=0; n<numClusters; n++){ // beginning cluster #n // first, count how many surfaces are in this cluster; if 1, things are easier int numThisCluster=0; int cluster1st=-1; for(size_t i=0; i<N; i++){ if(clusters[i]!=n) continue; numThisCluster++; if(cluster1st<0) cluster1st=(int)i; } GtsSurface* clusterSurf=NULL; LOG_DEBUG("Cluster "<<n<<" has "<<numThisCluster<<" surfaces."); if(numThisCluster==1){ clusterSurf=ssurf[cluster1st]; } else { clusterSurf=ssurf[cluster1st]; // surface of the cluster itself LOG_TRACE(" Initial cluster surface from "<<cluster1st<<"."); /* composed surface */ for(size_t i=0; i<N; i++){ if(clusters[i]!=n || ((int)i)==cluster1st) continue; LOG_TRACE(" Adding "<<i<<" to the cluster"); // ssurf[i] now belongs to cluster #n // trees need to be rebuild every time anyway, since the merged surface keeps changing in every cycle //if(gts_surface_face_number(clusterSurf)==0) LOG_ERROR("clusterSurf has 0 faces."); //if(gts_surface_face_number(ssurf[i])==0) LOG_ERROR("Surface #"<<i<<" has 0 faces."); GNode* t1=gts_bb_tree_surface(clusterSurf); GNode* t2=gts_bb_tree_surface(ssurf[i]); GtsSurfaceInter* I=gts_surface_inter_new(gts_surface_inter_class(),clusterSurf,ssurf[i],t1,t2,/*is_open_1*/false,/*is_open_2*/false); GtsSurface* merged=gts_surface_new(gts_surface_class(),gts_face_class(),gts_edge_class(),gts_vertex_class()); gts_surface_inter_boolean(I,merged,GTS_1_OUT_2); gts_surface_inter_boolean(I,merged,GTS_2_OUT_1); gts_object_destroy(GTS_OBJECT(I)); gts_bb_tree_destroy(t1,TRUE); gts_bb_tree_destroy(t2,TRUE); if(gts_surface_face_number(merged)==0){ LOG_ERROR("Cluster #"<<n<<": 0 faces after fusing #"<<i<<" (why?), adding #"<<i<<" separately!"); // this will cause an extra 1-particle cluster to be created clusters[i]=numClusters; numClusters+=1; } else { // not from global vectors (cleanup at the end), explicit delete! if(clusterSurf!=ssurf[cluster1st]) gts_object_destroy(GTS_OBJECT(clusterSurf)); clusterSurf=merged; } } } #if 0 LOG_TRACE(" GTS surface cleanups..."); pygts_vertex_cleanup(clusterSurf,.1*tol); // cleanup 10× smaller than tolerance pygts_edge_cleanup(clusterSurf); pygts_face_cleanup(clusterSurf); #endif LOG_TRACE(" STL: cluster "<<n<<" output"); stl<<"solid "<<solid<<"_"<<n<<"\n"; /* output cluster to STL here */ _gts_face_to_stl_data data(stl,scene,clipCell,numTri); gts_surface_foreach_face(clusterSurf,(GtsFunc)_gts_face_to_stl,(gpointer)&data); stl<<"endsolid\n"; if(clusterSurf!=ssurf[cluster1st]) gts_object_destroy(GTS_OBJECT(clusterSurf)); } // this deallocates also edges and vertices for(size_t i=0; i<ssurf.size(); i++) gts_object_destroy(GTS_OBJECT(ssurf[i])); return numTri; #endif /* WOO_GTS */ }
// Zhu et al. "A Rank-Order Distance based Clustering Algorithm for Face Tagging", CVPR 2011 br::Clusters br::ClusterGallery(const QStringList &simmats, float aggressiveness, const QString &csv) { qDebug("Clustering %d simmat(s)", simmats.size()); // Read in gallery parts, keeping top neighbors of each template Neighborhood neighborhood = getNeighborhood(simmats); const int cutoff = neighborhood.first().size(); const float threshold = 3*cutoff/4 * aggressiveness/5; // Initialize clusters Clusters clusters(neighborhood.size()); for (int i=0; i<neighborhood.size(); i++) clusters[i].append(i); bool done = false; while (!done) { // nextClusterIds[i] = j means that cluster i is set to merge into cluster j QVector<int> nextClusterIDs(neighborhood.size()); for (int i=0; i<neighborhood.size(); i++) nextClusterIDs[i] = i; // For each cluster for (int clusterID=0; clusterID<neighborhood.size(); clusterID++) { const Neighbors &neighbors = neighborhood[clusterID]; int nextClusterID = nextClusterIDs[clusterID]; // Check its neighbors foreach (const Neighbor &neighbor, neighbors) { int neighborID = neighbor.first; int nextNeighborID = nextClusterIDs[neighborID]; // Don't bother if they have already merged if (nextNeighborID == nextClusterID) continue; // Flag for merge if similar enough if (normalizedROD(neighborhood, clusterID, neighborID) < threshold) { if (nextClusterID < nextNeighborID) nextClusterIDs[neighborID] = nextClusterID; else nextClusterIDs[clusterID] = nextNeighborID; } } } // Transitive merge for (int i=0; i<neighborhood.size(); i++) { int nextClusterID = i; while (nextClusterID != nextClusterIDs[nextClusterID]) { assert(nextClusterIDs[nextClusterID] < nextClusterID); nextClusterID = nextClusterIDs[nextClusterID]; } nextClusterIDs[i] = nextClusterID; } // Construct new clusters QHash<int, int> clusterIDLUT; QList<int> allClusterIDs = QSet<int>::fromList(nextClusterIDs.toList()).values(); for (int i=0; i<neighborhood.size(); i++) clusterIDLUT[i] = allClusterIDs.indexOf(nextClusterIDs[i]); Clusters newClusters(allClusterIDs.size()); Neighborhood newNeighborhood(allClusterIDs.size()); for (int i=0; i<neighborhood.size(); i++) { int newID = clusterIDLUT[i]; newClusters[newID].append(clusters[i]); newNeighborhood[newID].append(neighborhood[i]); } // Update indices and trim for (int i=0; i<newNeighborhood.size(); i++) { Neighbors &neighbors = newNeighborhood[i]; int size = qMin(neighbors.size(),cutoff); std::partial_sort(neighbors.begin(), neighbors.begin()+size, neighbors.end(), compareNeighbors); for (int j=0; j<size; j++) neighbors[j].first = clusterIDLUT[j]; neighbors = neighbors.mid(0, cutoff); } // Update results done = true; //(newClusters.size() >= clusters.size()); clusters = newClusters; neighborhood = newNeighborhood; }
void RegionGrowingMultiplePlaneSegmentation::segment( const sensor_msgs::PointCloud2::ConstPtr& msg, const sensor_msgs::PointCloud2::ConstPtr& normal_msg) { boost::mutex::scoped_lock lock(mutex_); if (!done_initialization_) { return; } vital_checker_->poke(); { jsk_recognition_utils::ScopedWallDurationReporter r = timer_.reporter(pub_latest_time_, pub_average_time_); pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>); pcl::fromROSMsg(*msg, *cloud); pcl::fromROSMsg(*normal_msg, *normal); // concatenate fields pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr all_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::concatenateFields(*cloud, *normal, *all_cloud); pcl::PointIndices::Ptr indices (new pcl::PointIndices); for (size_t i = 0; i < all_cloud->points.size(); i++) { if (!isnan(all_cloud->points[i].x) && !isnan(all_cloud->points[i].y) && !isnan(all_cloud->points[i].z) && !isnan(all_cloud->points[i].normal_x) && !isnan(all_cloud->points[i].normal_y) && !isnan(all_cloud->points[i].normal_z) && !isnan(all_cloud->points[i].curvature)) { if (all_cloud->points[i].curvature < max_curvature_) { indices->indices.push_back(i); } } } pcl::ConditionalEuclideanClustering<pcl::PointXYZRGBNormal> cec (true); // vector of pcl::PointIndices pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters); cec.setInputCloud (all_cloud); cec.setIndices(indices); cec.setClusterTolerance(cluster_tolerance_); cec.setMinClusterSize(min_size_); //cec.setMaxClusterSize(max_size_); { boost::mutex::scoped_lock lock2(global_custom_condigion_function_mutex); setCondifionFunctionParameter(angular_threshold_, distance_threshold_); cec.setConditionFunction( &RegionGrowingMultiplePlaneSegmentation::regionGrowingFunction); //ros::Time before = ros::Time::now(); cec.segment (*clusters); // ros::Time end = ros::Time::now(); // ROS_INFO("segment took %f sec", (before - end).toSec()); } // Publish raw cluster information // pcl::IndicesCluster is typdefed as std::vector<pcl::PointIndices> jsk_recognition_msgs::ClusterPointIndices ros_clustering_result; ros_clustering_result.cluster_indices = pcl_conversions::convertToROSPointIndices(*clusters, msg->header); ros_clustering_result.header = msg->header; pub_clustering_result_.publish(ros_clustering_result); // estimate planes std::vector<pcl::PointIndices::Ptr> all_inliers; std::vector<pcl::ModelCoefficients::Ptr> all_coefficients; jsk_recognition_msgs::PolygonArray ros_polygon; ros_polygon.header = msg->header; for (size_t i = 0; i < clusters->size(); i++) { pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr cluster = boost::make_shared<pcl::PointIndices>((*clusters)[i]); ransacEstimation(cloud, cluster, *plane_inliers, *plane_coefficients); if (plane_inliers->indices.size() > 0) { jsk_recognition_utils::ConvexPolygon::Ptr convex = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZRGB>( cloud, plane_inliers, plane_coefficients); if (convex) { if (min_area_ > convex->area() || convex->area() > max_area_) { continue; } { // check direction consistency of coefficients and convex Eigen::Vector3f coefficient_normal(plane_coefficients->values[0], plane_coefficients->values[1], plane_coefficients->values[2]); if (convex->getNormalFromVertices().dot(coefficient_normal) < 0) { convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex()); } } // Normal should direct to origin { Eigen::Vector3f p = convex->getPointOnPlane(); Eigen::Vector3f n = convex->getNormal(); if (p.dot(n) > 0) { convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex()); Eigen::Vector3f new_normal = convex->getNormal(); plane_coefficients->values[0] = new_normal[0]; plane_coefficients->values[1] = new_normal[1]; plane_coefficients->values[2] = new_normal[2]; plane_coefficients->values[3] = convex->getD(); } } geometry_msgs::PolygonStamped polygon; polygon.polygon = convex->toROSMsg(); polygon.header = msg->header; ros_polygon.polygons.push_back(polygon); all_inliers.push_back(plane_inliers); all_coefficients.push_back(plane_coefficients); } } } jsk_recognition_msgs::ClusterPointIndices ros_indices; ros_indices.cluster_indices = pcl_conversions::convertToROSPointIndices(all_inliers, msg->header); ros_indices.header = msg->header; pub_inliers_.publish(ros_indices); jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients; ros_coefficients.header = msg->header; ros_coefficients.coefficients = pcl_conversions::convertToROSModelCoefficients( all_coefficients, msg->header); pub_coefficients_.publish(ros_coefficients); pub_polygons_.publish(ros_polygon); } }
KMeansMethodResult KMeansMethod::Clustering(const std::vector<std::vector<double>> &inputs, int dim, int countOfCluster) { int size = inputs.size(); std::vector<std::vector<double>> clusters(countOfCluster, std::vector<double>(dim, 0)); std::cout << "- Clustering -" << std::endl; // kMeans法++で初期値を決定 std::vector<int> clusterOfInputs = getInitialClusterOfInputs(inputs, dim, countOfCluster); std::cout << "initialized" << std::endl; while (true) { // 各クラスタの中心を求める for (int i = 0; i < countOfCluster; i++) { std::vector<double> center(dim, 0); int count = 0; // i番目のクラスタに含まれている入力を検索 for (int j = 0; j < size; j++) { if (clusterOfInputs[j] == i) { // クラスタの中心を求めるために加算 for (int k = 0; k < dim; k++) { center[k] += inputs[j][k]; } // 数を保存 count++; } } if (count > 0) { // クラスタの中心を求めるために数で割る for (int j = 0; j < dim; j++) { center[j] /= count; } // クラスタの決定 for (int j = 0; j < dim; j++) { clusters[i][j] = center[j]; } } } bool isChanged = false; // 入力をそれぞれ一番近いクラスタに付与 for (int i = 0; i < size; i++) { std::vector<double> input = inputs[i]; double minDistance = std::numeric_limits<double>::max(); int minIndex; // 一番近いクラスタを検索 for (int j = 0; j < countOfCluster; j++) { double distance = 0; std::vector<double> cluster = clusters[j]; for (int k = 0; k < dim; k++) { distance += pow(input[k] - cluster[k], 2.0); } if (distance < minDistance) { minDistance = distance; minIndex = j; } } // 距離が一番小さいところに置き換える if (clusterOfInputs[i] != minIndex) { clusterOfInputs[i] = minIndex; isChanged = true; } } // 変化がなければ処理を終了 if (isChanged == false) { break; } } // 一つも属されていないクラスタを削除する for (int i = countOfCluster - 1; i >= 0; i--) { int count = 0; for (int j = 0; j < size; j++) { if (clusterOfInputs[j] == i) { count++; } } if (count == 0) { clusters.erase(clusters.begin() + i); for (int j = 0; j < size; j++) { if (clusterOfInputs[j] > i) { clusterOfInputs[j] -= 1; } } } } std::cout << "clusters.size() -> " << clusters.size() << std::endl; KMeansMethodResult result; result.clusters = clusters; result.indexOfCluster = clusterOfInputs; return result; }
void CompleteLinkage::operator()(DistanceMatrix<float> & original_distance, std::vector<BinaryTreeNode> & cluster_tree, const float threshold /*=1*/) const { // attention: clustering process is done by clustering the indices // pointing to elements in inputvector and distances in inputmatrix // input MUST have >= 2 elements! if (original_distance.dimensionsize() < 2) { throw ClusterFunctor::InsufficientInput(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "Distance matrix to start from only contains one element"); } std::vector<std::set<Size> > clusters(original_distance.dimensionsize()); for (Size i = 0; i < original_distance.dimensionsize(); ++i) { clusters[i].insert(i); } cluster_tree.clear(); cluster_tree.reserve(original_distance.dimensionsize() - 1); // Initial minimum-distance pair original_distance.updateMinElement(); std::pair<Size, Size> min = original_distance.getMinElementCoordinates(); Size overall_cluster_steps(original_distance.dimensionsize()); startProgress(0, original_distance.dimensionsize(), "clustering data"); while (original_distance(min.first, min.second) < threshold) { //grow the tree cluster_tree.push_back(BinaryTreeNode(*(clusters[min.second].begin()), *(clusters[min.first].begin()), original_distance(min.first, min.second))); if (cluster_tree.back().left_child > cluster_tree.back().right_child) { std::swap(cluster_tree.back().left_child, cluster_tree.back().right_child); } if (original_distance.dimensionsize() > 2) { //pick minimum-distance pair i,j and merge them //pushback elements of second to first (and then erase second) clusters[min.second].insert(clusters[min.first].begin(), clusters[min.first].end()); // erase first one clusters.erase(clusters.begin() + min.first); //update original_distance matrix //complete linkage: new distance between clusters is the minimum distance between elements of each cluster //lance-williams update for d((i,j),k): 0.5* d(i,k) + 0.5* d(j,k) + 0.5* |d(i,k)-d(j,k)| for (Size k = 0; k < min.second; ++k) { float dik = original_distance.getValue(min.first, k); float djk = original_distance.getValue(min.second, k); original_distance.setValueQuick(min.second, k, (0.5f * dik + 0.5f * djk + 0.5f * std::fabs(dik - djk))); } for (Size k = min.second + 1; k < original_distance.dimensionsize(); ++k) { float dik = original_distance.getValue(min.first, k); float djk = original_distance.getValue(min.second, k); original_distance.setValueQuick(k, min.second, (0.5f * dik + 0.5f * djk + 0.5f * std::fabs(dik - djk))); } //reduce original_distance.reduce(min.first); //update minimum-distance pair original_distance.updateMinElement(); //get new min-pair min = original_distance.getMinElementCoordinates(); } else { break; } setProgress(overall_cluster_steps - original_distance.dimensionsize()); //repeat until only two cluster remains or threshold exceeded, last step skips matrix operations } //fill tree with dummy nodes Size sad(*clusters.front().begin()); for (Size i = 1; i < clusters.size() && (cluster_tree.size() < cluster_tree.capacity()); ++i) { cluster_tree.push_back(BinaryTreeNode(sad, *clusters[i].begin(), -1.0)); } //~ while(cluster_tree.size() < cluster_tree.capacity()) //~ { //~ cluster_tree.push_back(BinaryTreeNode(0,1,-1.0)); //~ } endProgress(); }
/** Compute the pose of the table plane * @param inputs * @param outputs * @return */ int process(const tendrils& inputs, const tendrils& outputs) { std::vector<tabletop_object_detector::TabletopObjectRecognizer<pcl::PointXYZ>::TabletopResult > results; // Process each table pose_results_->clear(); std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters_merged; // Map to store the transformation for each cluster (table_index) std::map<pcl::PointCloud<pcl::PointXYZ>::Ptr, size_t> cluster_table; std::vector<cv::Vec3f> translations(clusters_->size()); std::vector<cv::Matx33f> rotations(clusters_->size()); for (size_t table_index = 0; table_index < clusters_->size(); ++table_index) { getPlaneTransform((*table_coefficients_)[table_index], rotations[table_index], translations[table_index]); // Make the clusters be in the table frame size_t n_clusters = (*clusters_)[table_index].size(); std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters(n_clusters); cv::Matx33f Rinv = rotations[table_index].t(); cv::Vec3f Tinv = -Rinv*translations[table_index]; for (size_t cluster_index = 0; cluster_index < n_clusters; ++cluster_index) { clusters[cluster_index] = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>()); for(size_t i = 0; i < (*clusters_)[table_index][cluster_index].size(); ++i) { cv::Vec3f res = Rinv*(*clusters_)[table_index][cluster_index][i] + Tinv; clusters[cluster_index]->push_back(pcl::PointXYZ(res[0], res[1], res[2])); } cluster_table.insert(std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, size_t>(clusters[cluster_index], table_index)); } clusters_merged.insert(clusters_merged.end(), clusters.begin(), clusters.end()); } object_recognizer_.objectDetection(clusters_merged, confidence_cutoff_, perform_fit_merge_, results); for (size_t i = 0; i < results.size(); ++i) { const tabletop_object_detector::TabletopObjectRecognizer<pcl::PointXYZ>::TabletopResult & result = results[i]; const size_t table_index = cluster_table[result.cloud_]; PoseResult pose_result; // Add the object id std::stringstream ss; ss << result.object_id_; pose_result.set_object_id(db_, ss.str()); // Add the pose const geometry_msgs::Pose &pose = result.pose_; cv::Vec3f T(pose.position.x, pose.position.y, pose.position.z); Eigen::Quaternionf quat(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); cv::Vec3f new_T = rotations[table_index] * T + translations[table_index]; pose_result.set_T(cv::Mat(new_T)); pose_result.set_R(quat); cv::Mat R = cv::Mat(rotations[table_index] * pose_result.R<cv::Matx33f>()); pose_result.set_R(R); pose_result.set_confidence(result.confidence_); // Add the cluster of points std::vector<sensor_msgs::PointCloud2ConstPtr> ros_clouds (1); sensor_msgs::PointCloud2Ptr cluster_cloud (new sensor_msgs::PointCloud2()); #if PCL_VERSION_COMPARE(>=,1,7,0) ::pcl::PCLPointCloud2 pcd_tmp; ::pcl::toPCLPointCloud2(*result.cloud_, pcd_tmp); pcl_conversions::fromPCL(pcd_tmp, *cluster_cloud); #else pcl::toROSMsg(*result.cloud_, *cluster_cloud); #endif ros_clouds[0] = cluster_cloud; pose_result.set_clouds(ros_clouds); pose_results_->push_back(pose_result); } return ecto::OK; }
bool MinDist::loadLegacyModelFromFile( fstream &file ){ string word; file >> word; if(word != "NumFeatures:"){ errorLog << "loadModelFromFile(string filename) - Could not find NumFeatures " << endl; return false; } file >> numInputDimensions; file >> word; if(word != "NumClasses:"){ errorLog << "loadModelFromFile(string filename) - Could not find NumClasses" << endl; return false; } file >> numClasses; file >> word; if(word != "UseScaling:"){ errorLog << "loadModelFromFile(string filename) - Could not find UseScaling" << endl; return false; } file >> useScaling; file >> word; if(word != "UseNullRejection:"){ errorLog << "loadModelFromFile(string filename) - Could not find UseNullRejection" << endl; return false; } file >> useNullRejection; ///Read the ranges if needed if( useScaling ){ //Resize the ranges buffer ranges.resize(numInputDimensions); file >> word; if(word != "Ranges:"){ errorLog << "loadModelFromFile(string filename) - Could not find the Ranges" << endl; return false; } for(UINT n=0; n<ranges.size(); n++){ file >> ranges[n].minValue; file >> ranges[n].maxValue; } } //Resize the buffer models.resize(numClasses); classLabels.resize(numClasses); //Load each of the K models for(UINT k=0; k<numClasses; k++){ double rejectionThreshold; double gamma; double trainingSigma; double trainingMu; file >> word; if( word != "ClassLabel:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the class label for class " << k << endl; return false; } file >> classLabels[k]; file >> word; if( word != "NumClusters:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the NumClusters for class " << k << endl; return false; } file >> numClusters; file >> word; if( word != "RejectionThreshold:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the RejectionThreshold for class " << k << endl; return false; } file >> rejectionThreshold; file >> word; if( word != "Gamma:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the Gamma for class " << k << endl; return false; } file >> gamma; file >> word; if( word != "TrainingMu:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the TrainingMu for class " << k << endl; return false; } file >> trainingMu; file >> word; if( word != "TrainingSigma:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the TrainingSigma for class " << k << endl; return false; } file >> trainingSigma; file >> word; if( word != "ClusterData:" ){ errorLog << "loadModelFromFile(string filename) - Could not load the ClusterData for class " << k << endl; return false; } //Load the cluster data MatrixDouble clusters(numClusters,numInputDimensions); for(UINT i=0; i<numClusters; i++){ for(UINT j=0; j<numInputDimensions; j++){ file >> clusters[i][j]; } } models[k].setClassLabel( classLabels[k] ); models[k].setClusters( clusters ); models[k].setGamma( gamma ); models[k].setRejectionThreshold( rejectionThreshold ); models[k].setTrainingSigma( trainingSigma ); models[k].setTrainingMu( trainingMu ); } //Recompute the null rejection thresholds recomputeNullRejectionThresholds(); //Resize the prediction results to make sure it is setup for realtime prediction maxLikelihood = DEFAULT_NULL_LIKELIHOOD_VALUE; bestDistance = DEFAULT_NULL_DISTANCE_VALUE; classLikelihoods.resize(numClasses,DEFAULT_NULL_LIKELIHOOD_VALUE); classDistances.resize(numClasses,DEFAULT_NULL_DISTANCE_VALUE); trained = true; return true; }
void HAC::train(Matrix& features, Matrix& labels) { _instanceToCluster = std::vector<size_t>(features.rows()); _adjMatrix = std::vector<std::vector<double> >(features.rows(), std::vector<double>(features.rows(), 0)); for(size_t i = 0; i < features.rows(); i++) { _instanceToCluster[i] = i; for(size_t j = 0; j < features.rows(); j++) { double dist = 0; if(i != j) dist = ClusteringUtils::dist(features, i, j); _adjMatrix[i][j] = dist; } } size_t numClusters = features.rows(); while(numClusters > _numClusters) { size_t cluster0 = 0; size_t cluster1 = 0; #if SINGLE_LINK // find the min-dist (single link) double minDist = DBL_MAX; for(size_t i = 0; i < features.rows(); i++) { for(size_t j = i + 1; j < features.rows(); j++) { if(_adjMatrix[i][j] < minDist && _instanceToCluster[i] != _instanceToCluster[j]) { minDist = _adjMatrix[i][j]; cluster0 = _instanceToCluster[i]; cluster1 = _instanceToCluster[j]; } } } if(_logOn) { std::cout << "Merging clusters: " << cluster0 << " and " << cluster1 << std::endl; std::cout << "Distance: " << minDist << std::endl; } #else // complete link (find the minimum largest distance between clusters) std::vector<std::vector<double> > clusterDistances(numClusters, std::vector<double>(numClusters, DBL_MIN)); for(size_t i = 0; i < features.rows(); i++) { for(size_t j = i + 1; j < features.rows(); j++) { if(_instanceToCluster[i] == _instanceToCluster[j]) continue; size_t lowClusterIndex = std::min(_instanceToCluster[i],_instanceToCluster[j]); size_t highClusterIndex = std::max(_instanceToCluster[i],_instanceToCluster[j]); if(_adjMatrix[i][j] > clusterDistances[lowClusterIndex][highClusterIndex]) clusterDistances[lowClusterIndex][highClusterIndex] = _adjMatrix[i][j]; } } double minLargestDist = DBL_MAX; for(size_t i = 0; i < clusterDistances.size(); i++) { for(size_t j = i + 1; j < clusterDistances.size(); j++) { if(clusterDistances[i][j] < minLargestDist) { minLargestDist = clusterDistances[i][j]; cluster0 = i; cluster1 = j; } } } if(_logOn) { std::cout << "Merging clusters: " << cluster0 << " and " << cluster1 << std::endl; std::cout << "Distance: " << minLargestDist << std::endl; } #endif // group clusters assert(cluster0 != cluster1); for(size_t i = 0; i < _instanceToCluster.size(); i++) { if(_instanceToCluster[i] == cluster1) _instanceToCluster[i] = cluster0; } std::cout << std::endl; normalizeLabels(); numClusters -= 1; } std::vector<Matrix> clusters(_numClusters, features); for(size_t i = 0; i < _instanceToCluster.size(); i++) { size_t clusterIndex = _instanceToCluster[i]; clusters[clusterIndex].copyRow(features[i]); } // calculate centroids std::vector<std::vector<double> > clusterMeans(_numClusters, std::vector<double>(features.cols())); for(size_t i = 0; i < _numClusters; i++) { for(size_t j = 0; j < features.cols(); j++) { if(features.valueCount(j) == 0) // continuous data clusterMeans[i][j] = clusters[i].columnMean(j); else clusterMeans[i][j] = clusters[i].mostCommonValue(j); } if(_logOn) { std::cout << "Centroid " << i << ": "; std::cout << getInstanceString(clusterMeans[i], features) << std::endl; } } // calculate SSE double sse = 0.0; for(size_t i = 0; i < _instanceToCluster.size(); i++) { double d = ClusteringUtils::dist(features, i, clusterMeans[_instanceToCluster[i]]); sse += d * d; } if(_logOn) { std::cout << "Total sse: " << sse << std::endl; } if(_logOn) { std::vector<Matrix> clusters(_numClusters, features); for(size_t i = 0; i < _instanceToCluster.size(); i++) clusters[_instanceToCluster[i]].copyRow(features[i]); ClusteringUtils::outputClusterStats(clusters, std::cout); } }