nearest_neighbors_graph_builder.hpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2014-, Open Perception, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */
 
#ifndef PCL_GRAPH_IMPL_NEAREST_NEIGHBORS_GRAPH_BUILDER_CPP
#define PCL_GRAPH_IMPL_NEAREST_NEIGHBORS_GRAPH_BUILDER_CPP
 
#include <pcl/common/io.h>
#include <pcl/common/point_tests.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/organized.h>
 
#include "nearest_neighbors_graph_builder.h"
 
template <typename PointT, typename GraphT>
void
pcl::graph::NearestNeighborsGraphBuilder<PointT, GraphT>::compute(GraphT& graph)
{
    if (!initCompute())
    {
        graph = GraphT();
        deinitCompute();
        return;
    }
 
    size_t k = 0;
    for (size_t i = 0; i < indices_->size(); ++i)
    {
        if (!pcl::isFinite(input_->operator[](indices_->operator[](i))))
        {
            fake_indices_ = false;
        }
        else
        {
            indices_->operator[](k++) = indices_->operator[](i);
        }
    }
    indices_->resize(k);
 
    // Create a new point cloud which will be the basis for the constructed graph.
    // All the fields that are also present in the output point type will be
    // copied over from the original point cloud.
    typename pcl::PointCloud<PointOutT>::Ptr cloud(new pcl::PointCloud<PointOutT>);
    pcl::copyPointCloud(*input_, *indices_, *cloud);
    graph = GraphT(cloud);
 
    // In case a search method has not been given, initialize it using defaults
    if (!search_)
    {
        // For organized datasets, use OrganizedNeighbor
        if (cloud->isOrganized())
        {
            search_.reset(new pcl::search::OrganizedNeighbor<PointOutT>);
        }
        // For unorganized data, use KdTree
        else
        {
            search_.reset(new pcl::search::KdTree<PointOutT>);
        }
    }
 
    // Establish edges with nearest neighbors.
    std::vector<int> neighbors(num_neighbors_ + 1);
    std::vector<float> distances(num_neighbors_ + 1);
    search_->setInputCloud(cloud);
    for (size_t i = 0; i < cloud->size(); ++i)
    {
        // Search for num_neighbors_ + 1 because the first neighbor output by KdTree
        // is always the query point itself.
        search_->nearestKSearch(i, num_neighbors_ + 1, neighbors, distances);
        for (size_t j = 1; j < neighbors.size(); ++j)
            if (!boost::edge(i, neighbors[j], graph).second)
            {
                boost::add_edge(i, neighbors[j], graph);
            }
    }
 
    // Create point to vertex map
    point_to_vertex_map_.resize(input_->size(), std::numeric_limits<VertexId>::max());
    VertexId v = 0;
    for (size_t i = 0; i < indices_->size(); ++i)
    {
        point_to_vertex_map_[indices_->operator[](i)] = v++;
    }
}
 
#endif /* PCL_GRAPH_IMPL_NEAREST_NEIGHBORS_GRAPH_BUILDER_HPP */