edge_weight_computer.h

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_EDGE_WEIGHT_COMPUTER_H
#define PCL_GRAPH_EDGE_WEIGHT_COMPUTER_H
 
#include <boost/function.hpp>
 
#include "point_cloud_graph.h"
#include "point_cloud_graph_concept.h"
 
#include "edge_weight_computer_terms.h"
 
namespace pcl::graph
{
 
    template <typename GraphT>
    class PCL_EXPORTS EdgeWeightComputer
    {
 
        BOOST_CONCEPT_ASSERT((pcl::graph::PointCloudGraphConcept<GraphT>));
 
    public:
 
        typedef typename pcl::graph::point_cloud_graph_traits<GraphT>::point_type PointT;
        typedef boost::function<float (float, float)> TermBalancingFunction;
        typedef boost::shared_ptr<EdgeWeightComputer> Ptr;
 
        enum NormalizationType
        {
            NORMALIZATION_NONE,
            NORMALIZATION_GLOBAL,
            NORMALIZATION_LOCAL
        };
 
        enum SmallWeightPolicy
        {
            SMALL_WEIGHT_IGNORE,
            SMALL_WEIGHT_COERCE_TO_THRESHOLD,
            SMALL_WEIGHT_REMOVE_EDGE,
        };
 
        EdgeWeightComputer()
            : policy_(SMALL_WEIGHT_IGNORE)
            , threshold_(0.0f)
            , balancing_function_(&EdgeWeightComputer<GraphT>::gaussian)
        {
        }
 
        template <typename EdgeWeightMap> void
        compute(GraphT& graph, EdgeWeightMap weights);
 
        inline void
        compute(GraphT& graph)
        {
            compute(graph, boost::get(boost::edge_weight, graph));
        }
 
        template <typename TermT> void
        addTerm(float influence,
                float convex_influence_multiplier = 1.0,
                NormalizationType normalization = NORMALIZATION_NONE)
        {
            addTermImpl<TermT> (influence, convex_influence_multiplier, normalization,
                                typename boost::mpl::apply<typename TermT::is_compatible, PointT>::type());
        }
 
        template <typename TermT> void
        addTerm(float influence,
                NormalizationType normalization)
        {
            addTermImpl<TermT> (influence, 1.0, normalization,
                                typename boost::mpl::apply<typename TermT::is_compatible, PointT>::type());
        }
 
        inline void
        setSmallWeightPolicy(SmallWeightPolicy policy)
        {
            policy_ = policy;
        }
 
        inline void
        setSmallWeightThreshold(float threshold)
        {
            threshold_ = threshold;
        }
 
        inline void
        setTermBalancingFunction(TermBalancingFunction func)
        {
            balancing_function_ = func;
        }
 
    private:
 
        static float
        gaussian(float val, float influence)
        {
            return (influence > 0.0 ? std::exp(-val / influence) : 1.0);
        };
 
        struct Term
        {
 
            typedef boost::function<float (const PointT&, const PointT&)> ComputeFunction;
 
            ComputeFunction compute_;
            float influence_;
            float convex_influence_multiplier_;
 
            Term(ComputeFunction f, float i, float c)
                : compute_(f)
                , influence_(i)
                , convex_influence_multiplier_(c)
            {
            }
 
            inline float
            getInfluence(bool is_convex = false) const
            {
                return (influence_ * (is_convex ? convex_influence_multiplier_ : 1.0));
            }
 
        };
 
        struct GloballyNormalizedTerm : Term
        {
 
            GloballyNormalizedTerm(typename Term::ComputeFunction f, float i, float c)
                : Term(f, i, c)
            {
            }
 
            void
            init(size_t num_edges)
            {
                edge_weights_.resize(num_edges, 0.0f);
                total_weight_ = 0.0f;
            }
 
            float
            round1(const PointT& p1, const PointT& p2, size_t edge_id)
            {
                float weight = this->compute_(p1, p2);
                edge_weights_[edge_id] = weight;
                total_weight_ += weight;
                return (weight);
            }
 
            void
            extract()
            {
                average_ = total_weight_ / edge_weights_.size();
            }
 
            float
            round2(size_t edge_id)
            {
                return (edge_weights_[edge_id] / average_);
            }
 
            std::vector<float> edge_weights_;
            float total_weight_;
            float average_;
 
        };
 
        struct LocallyNormalizedTerm : Term
        {
 
            LocallyNormalizedTerm(typename Term::ComputeFunction f, float i, float c)
                : Term(f, i, c)
            {
            }
 
            void
            init(size_t num_edges, size_t num_vertices)
            {
                edge_weights_.resize(num_edges, 0.0f);
                vertex_sums_.resize(num_vertices, 0.0f);
                vertex_degrees_.resize(num_vertices, 0);
            }
 
            float
            round1(const PointT& p1,
                   const PointT& p2,
                   size_t vertex1_id,
                   size_t vertex2_id,
                   size_t edge_id)
            {
                float weight = this->compute_(p1, p2);
                edge_weights_[edge_id] = weight;
                vertex_sums_[vertex1_id] += weight;
                vertex_sums_[vertex2_id] += weight;
                ++vertex_degrees_[vertex1_id];
                ++vertex_degrees_[vertex2_id];
                return (weight);
            }
 
            void
            extract()
            {
                for (size_t i = 0; i < vertex_sums_.size(); ++i)
                    if (vertex_degrees_[i])
                    {
                        vertex_sums_[i] /= vertex_degrees_[i];
                    }
            }
 
            float
            round2(size_t vertex1_id,
                   size_t vertex2_id,
                   size_t edge_id) const
            {
                float n = (vertex_sums_[vertex1_id] + vertex_sums_[vertex2_id]) / 2.0;
                float weight = (n > 0.0f && this->getInfluence() > 0.0f)
                               ? edge_weights_[edge_id] / n
                               : 0.0f;
                return (weight);
            }
 
            std::vector<float> edge_weights_;
            std::vector<float> vertex_sums_;
            std::vector<size_t> vertex_degrees_;
 
        };
 
        template <typename TermT> void
        addTermImpl(float influence,
                    float convex_influence_multiplier,
                    NormalizationType normalization,
                    boost::mpl::bool_<true>)
        {
            switch (normalization)
            {
                case NORMALIZATION_NONE:
                {
                    terms_.push_back(Term(TermT::template compute<PointT>, influence, convex_influence_multiplier));
                    break;
                }
                case NORMALIZATION_GLOBAL:
                {
                    g_terms_.push_back(GloballyNormalizedTerm(TermT::template compute<PointT>, influence, convex_influence_multiplier));
                    break;
                }
                case NORMALIZATION_LOCAL:
                {
                    l_terms_.push_back(LocallyNormalizedTerm(TermT::template compute<PointT>, influence, convex_influence_multiplier));
                    break;
                }
            }
        }
 
        template <typename TermT> void
        addTermImpl(float influence,
                    float convex_influence_multiplier,
                    NormalizationType normalization,
                    boost::mpl::bool_<false>)
        {
        }
 
        std::vector<Term> terms_;
        std::vector<GloballyNormalizedTerm> g_terms_;
        std::vector<LocallyNormalizedTerm> l_terms_;
 
        SmallWeightPolicy policy_;
        float threshold_;
        TermBalancingFunction balancing_function_;
 
    };
 
}
 
 
#include "edge_weight_computer.hpp"
 
#endif /* PCL_GRAPH_EDGE_WEIGHT_COMPUTER_H */