DenseGraphCRF.hpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ROBDEKON::ArmarXObjects::DenseCRFSegmentationProcessor
 * @author     Christoph Pohl ( christoph dot pohl at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#ifndef ROBDEKON_DENSEGRAPHCRF_HPP
#define ROBDEKON_DENSEGRAPHCRF_HPP
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <densecrf.h>
#include <pairwise.h>
#include <permutohedral.h>
#include <objective.h>
 
 
#include <VisionX/libraries/PointCloudGraph/PointXYZRGBLNormal.h>
 
 
#include <pcl/point_types.h>
 
#include "DenseCRFFeatureTerms.h"
#include "Common.h"
 
 
namespace armarx
{
    template <class GraphT>
    class DenseGraphCRF : private DenseCRF
    {
    private:
        // int N_ from base class == Number of Vertices in the Graph
        // int M_ from base class == Number of Classes
        typedef boost::shared_ptr<GraphT> GraphPtrT;
        typedef typename boost::graph_traits<GraphT>::vertex_descriptor VtxId;
        GraphPtrT _graph;
 
    public:
        DenseGraphCRF(GraphPtrT Graph, int M);
 
 
        template <typename FeatureT>
        inline void addPairwiseGaussianFeature(std::vector<float> sigma, LabelCompatibility* function,
                                               KernelType kernel_type = DIAG_KERNEL,
                                               NormalizationType normalization_type = NORMALIZE_SYMMETRIC)
        {
            ARMARX_TRACE;
            FeatureT functor(*_graph);
            Eigen::MatrixXf features = functor.computeFeatures(sigma);
            addPairwiseEnergy(features, function, kernel_type, normalization_type);
        }
 
        inline void
        addCombinedGaussianFeature(std::vector<float> sigma, bool use_rgb, bool use_norm, bool use_xyz, bool use_curv,
                                   bool use_time, LabelCompatibility* function, KernelType kernel_type = DIAG_KERNEL,
                                   NormalizationType normalization_type = NORMALIZE_SYMMETRIC)
        {
            ARMARX_TRACE;
            VariableCombinedNormalizedFeature functor(*_graph, use_rgb, use_norm, use_xyz, use_curv, use_time);
            Eigen::MatrixXf features = functor.computeFeatures(sigma);
            addPairwiseEnergy(features, function, kernel_type, normalization_type);
        }
 
        void computeUnaryEnergyFromGraph(float gt_prob);
 
        void map(int n_iterations);
 
        Eigen::VectorXf map_and_confidence(int n_iterations);
        //        void addPairwiseGaussianVector();
    };
 
    template <class GraphT>
    DenseGraphCRF<GraphT>::DenseGraphCRF(GraphPtrT Graph, int M) : DenseCRF(
            static_cast<int>(boost::num_vertices(*Graph)), M), _graph(Graph)
    {
 
    }
 
    template <class GraphT>
    void DenseGraphCRF<GraphT>::computeUnaryEnergyFromGraph(float gt_prob)
    {
        ARMARX_TRACE;
        Eigen::MatrixXf energy(M_, N_);
        ARMARX_CHECK_EXPRESSION(static_cast<int>(boost::num_vertices(*_graph)) == N_);
        const float u_energy = -log(1.0 / M_);
        const float n_energy = -log((1.0 - gt_prob) / (M_ - 1));
        const float p_energy = -log(gt_prob);
        energy.fill(u_energy);
        for (int k = 0; k < N_; k++)
        {
            // Set the energy
            int r = (*_graph)[static_cast<VtxId >(k)].label;
            if (r >= 0)
            {
                ARMARX_TRACE;
                energy.col(k).fill(n_energy);
                energy(r, k) = p_energy;
            }
        }
        ARMARX_TRACE;
        setUnaryEnergy(energy);
    }
 
    template <class GraphT>
    void DenseGraphCRF<GraphT>::map(int n_iterations)
    {
        ARMARX_TRACE;
        VectorXs map_result = DenseCRF::map(n_iterations);
        const int res_size = map_result.size();
        ARMARX_CHECK_EXPRESSION(res_size == static_cast<int>(boost::num_vertices(*_graph)));
        for (int i = 0; i < res_size; i++)
        {
            (*_graph)[static_cast<VertexId>(i)].label = map_result[i];
        }
    }
 
    template <class GraphT>
    Eigen::VectorXf DenseGraphCRF<GraphT>::map_and_confidence(int n_iterations)
    {
        ARMARX_TRACE;
        Eigen::MatrixXf Q = inference(n_iterations);
        // Find the map - VectorXs
        Eigen::Matrix<short, Eigen::Dynamic, 1> map_result = currentMap(Q);
        const int res_size = map_result.size();
        ARMARX_CHECK_EXPRESSION(res_size == static_cast<int>(boost::num_vertices(*_graph)));
        Eigen::VectorXf confidence_score;
        ConfidenceMap cm = boost::get(boost::vertex_confidence_t(), _graph->m_graph);
 
        confidence_score.resize(res_size);
        for (int i = 0; i < res_size; i++)
        {
            (*_graph)[static_cast<VtxId>(i)].label = map_result[i];
            boost::put(cm, static_cast<VtxId>(i), Q(map_result[i], i));
            confidence_score[i] = Q(map_result[i], i);
        }
        return confidence_score;
    }
}
 
 
#endif //ROBDEKON_DENSEGRAPHCRF_HPP