Beispiel #1
0
 inline Eigen::Matrix<T,1,C> 
 columns_dot_self(const Eigen::Matrix<T,R,C>& x) {
   return x.colwise().squaredNorm();
 }
Beispiel #2
-1
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;
}