kzampog / cilantro

A lean C++ library for working with point cloud data
MIT License
1.01k stars 206 forks source link

KDTree Search problem! #51

Closed CarlLyt closed 4 years ago

CarlLyt commented 4 years ago

Hi I need a short guide with cilantro . T.T I follow example/kd_tree.cpp and there is my code

        cilantro::PointCloud3f icp_out = src.transformed(tf_est);
        cilantro::Timer tree_timer;
        tree_timer.start();
        cilantro::KDTree3f<> tree(icp_out.points);
        for(int k = 0;k<dst.size();k++)
        {
            cilantro::NeighborSet<float> nn = tree.kNNSearch(Eigen::Vector3f(icp_out.points(k,0), icp_out.points(k,1), icp_out.points(k,2)), 1);

            std::cout<<"search::  "<< k<<std::endl;
            //std::cout << Eigen::Vector3f(icp_out.points(k,0), icp_out.points(k,1), icp_out.points(k,2)) <<std::endl;
            std::cout << "Neighbor indices: ";
//            for (int m= 0; m < nn.siz
            std::cout<<"result::";

            std::cout << nn[0].index << std::endl;
            //std::cout << Eigen::Vector3f(icp_out.points(nn[0].index,0), icp_out.points(nn[0].index,1), icp_out.points(nn[0].index,2)) <<std::endl;
            std::cout << std::endl;

            std::cout << "Neighbor distances: ";
            std::cout << nn[0].value << " ";
            std::cout << std::endl;

        }
        tree_timer.stop();

I build a tree with icp_out and i search with icp_out,I want to get icp_out.points(k,0) own subscript k to verify the correctness of kdtree, But no matter what my input is, the index output will always be 434 and 663 image What mistake did I make? please~

CarlLyt commented 4 years ago

sorry ,my mistake, cilantro::PointCloud3f is a 3*N vector,