inline Eigen::Matrix<T,1,C> columns_dot_self(const Eigen::Matrix<T,R,C>& x) { return x.colwise().squaredNorm(); }
typename k_means_tree<Point, K, Data, Lp>::leaf_range k_means_tree<Point, K, Data, Lp>::assign_nodes(CloudPtrT& subcloud, node** nodes, size_t current_depth, const vector<int>& subinds) { std::cout << "Now doing level " << current_depth << std::endl; std::cout << subcloud->size() << std::endl; // do k-means of the points, iteratively call this again? //PointT centroids[dim]; Eigen::Matrix<float, rows, dim> centroids; Eigen::Matrix<float, rows, dim> last_centroids; Eigen::Matrix<float, 1, dim> distances; // first, pick centroids at random vector<size_t> inds = sample_with_replacement(subcloud->size()); for (size_t i = 0; i < dim; ++i) { //centroids[i] = subcloud->points[inds[i]]; centroids.col(i) = eig(subcloud->points[inds[i]]); } last_centroids.setZero(); // if there are no more than 1000 points, continue as normal, // otherwise decrease to about a 1000 points then double with every iteration int skip = std::max(1 << int(log2(double(subcloud->size())/1000.0)), 1); std::vector<int> clusters[dim]; //float cluster_distances[dim]; size_t min_iter = std::max(50, int(subcloud->size()/100)); // 50 100 size_t counter = 0; PointT p; while (true) { // compute closest centroids for (std::vector<int>& c : clusters) { c.clear(); } //int ind = 0; //for (const PointT& p : subcloud->points) { int _subcloud_size = subcloud->size(); for (int ind = 0; ind < _subcloud_size; ind += skip) { p = subcloud->at(ind); int closest; // Wrap these two calls with some nice inlining //distances = eig(p).transpose()*centroids; distances = (centroids.colwise()-eig(p)).array().abs().colwise().sum(); //distances = (centroids.colwise()-eig(p)).colwise().squaredNorm(); distances.minCoeff(&closest); clusters[closest].push_back(ind); } if (skip == 1 && (counter >= min_iter || compare_centroids(centroids, last_centroids))) { break; } last_centroids = centroids; // compute new centroids for (size_t i = 0; i < dim; ++i) { Eigen::Matrix<double, rows, 1> acc; acc.setZero(); size_t nbr = 0; for (size_t ind : clusters[i]) { acc += eig(subcloud->at(ind)).template cast<double>(); ++nbr; } if (nbr == 0) { vector<size_t> temp = sample_with_replacement(subcloud->size()); //centroids[i] = subcloud->at(temp.back()); centroids.col(i) = eig(subcloud->at(temp.back())); } else { acc *= 1.0/double(nbr); //eig(centroids[i]) = acc.template cast<float>(); centroids.col(i) = acc.template cast<float>(); } } skip = std::max(skip/2, 1); ++counter; } leaf_range range(cloud->size(), 0); for (size_t i = 0; i < dim; ++i) { //std::cout << i << " size: " << clusters[i].size() << std::endl; if (current_depth == depth || clusters[i].size() <= 1) { leaf* l = new leaf; l->inds.resize(clusters[i].size()); for (size_t j = 0; j < clusters[i].size(); ++j) { l->inds[j] = subinds[clusters[i][j]]; } //l->centroid = centroids[i]; eig(l->centroid) = centroids.col(i); /*if (clusters[i].empty()) { eig(l->centroid).setZeros(); }*/ l->range.first = leaves.size(); l->range.second = leaves.size()+1; leaves.push_back(l); nodes[i] = l; range.first = std::min(range.first, l->range.first); range.second = std::max(range.second, l->range.second); continue; } node* n = new node; //n->centroid = centroids[i]; eig(n->centroid) = centroids.col(i); CloudPtrT childcloud(new CloudT); childcloud->resize(clusters[i].size()); vector<int> childinds(clusters[i].size()); for (size_t j = 0; j < clusters[i].size(); ++j) { childcloud->at(j) = subcloud->at(clusters[i][j]); childinds[j] = subinds[clusters[i][j]]; } leaf_range rangei = assign_nodes(childcloud, n->children, current_depth+1, childinds); n->range = rangei; nodes[i] = n; range.first = std::min(range.first, rangei.first); range.second = std::max(range.second, rangei.second); /*pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); inliers->indices = clusters[i]; pcl::ExtractIndices<PointT> extract; extract.setInputCloud(subcloud); extract.setIndices(inliers); extract.setNegative(false); extract.filter (*childcloud);*/ } return range; }