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;
}