CGAL nearest neighbor search from Point_set_3 with properties

main.cpp

#include <array>
#include <iostream>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Point_set_3.h>

#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>

template < typename CloudT, typename ColorMapT >
typename boost::property_traits<ColorMapT>::value_type nearest_color(
    const CloudT& cloud,
    const ColorMapT& color_map,
    const typename CloudT::Point_3& query) {
    
    typedef typename boost::property_traits<ColorMapT>::value_type Color_t;
    
    typedef typename CGAL::Kernel_traits<typename CloudT::Point_3>::Kernel CloudKernel_t;
    typedef CGAL::Search_traits_3<CloudKernel_t> SearchTraits_base_t;
    typedef CGAL::Search_traits_adapter< 
        typename CloudT::Index, 
        typename CloudT::Point_map, 
        SearchTraits_base_t 
    > SearchTraits_t;

    typedef CGAL::Orthogonal_k_neighbor_search<SearchTraits_t> CloudSearch_t;
    typedef typename CloudSearch_t::Tree                       CloudSearchTree_t;
    typedef typename CloudSearchTree_t::Splitter               CloudSearchSplitter_t;
    typedef typename CloudSearch_t::Distance                   CloudSearchDistance_t;

    // Set up a search tree.
    CloudSearchTree_t tree( 
        cloud.begin(), cloud.end(), 
        CloudSearchSplitter_t(),
        SearchTraits_t( cloud.point_map() ) );

    // Search.
    CloudSearchDistance_t distance( cloud.point_map() );
    CloudSearch_t search( tree, query, 1, 0, true, distance );

    const auto& neighbor_point_index = search.begin()->first;
    std::cout << "Found nearest point of " << query <<": \n"
              << "index = " << neighbor_point_index << ", "
              << "distance = " << search.begin()->second << ", "
              << "point: [" << cloud.point(neighbor_point_index) << " ]"
              << "\n";

    auto& cloud_neighbor_color = color_map[neighbor_point_index];

    return cloud_neighbor_color;
}


int main(int argc, char** argv) {
    std::cout << "Hello, test_point_set_nearest_neighbor! \n";

    typedef std::array< unsigned char, 3 > Color_t;
    typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel_t;
    typedef Kernel_t::Point_3 Point_t;
    typedef CGAL::Point_set_3<Point_t> PointSet_t;

    PointSet_t cloud;

    auto [ color_map, flag_added ] = cloud.add_property_map<Color_t>("color");

    for ( int i = 0; i < 10; ++i ) {
        PointSet_t::iterator it = cloud.insert( Point_t( i*0.1, i*0.1, i*0.1 ) );
        color_map[*it] = {{ 
            static_cast<unsigned char>(100+i), 
            static_cast<unsigned char>(100+i), 
            static_cast<unsigned char>(100+i)
        }};
    }

    for ( const auto& index : cloud ) {
        const auto& point = cloud.point(index);
        const auto& color = color_map[index];

        std::cout << "index = " << index << ", "
                  << "point: [ " << point << " ], "
                  << "color: [ "
                  << static_cast<int>(color[0]) << ", "
                  << static_cast<int>(color[1]) << ", "
                  << static_cast<int>(color[2]) << " ]\n";
    }

    // auto color = nearest_color( cloud, color_map, Point_t(0.45, 0.45, 0.45) );
    auto color = nearest_color( cloud, color_map, Point_t(0., 0., 0.) );
    std::cout << "color = [ " 
              << static_cast<int>(color[0]) << ", "
              << static_cast<int>(color[1]) << ", "
              << static_cast<int>(color[2]) << " ]\n";
    
    return 0;
}