DenseCRFSegmentationProcessor.h

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 _ARMARX_COMPONENT_ROBDEKON_DenseCRFSegmentationProcessor_H
#define _ARMARX_COMPONENT_ROBDEKON_DenseCRFSegmentationProcessor_H
 
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/CPPUtility/ConfigIntrospection/create_macro.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
 
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
// RemoteGUI
#include <boost/graph/filtered_graph.hpp>
 
#include <pcl/point_types.h>
 
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/RemoteGuiComponentPlugin.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
 
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
#include <VisionX/interface/components/DenseCRFSegmentationProcessorInterface.h>
#include <VisionX/libraries/PointCloudGraph/PointXYZRGBLNormal.h>
#include <VisionX/libraries/PointCloudGraph/common.h>
#include <VisionX/libraries/PointCloudGraph/point_cloud_graph.h>
#include <VisionX/libraries/PointCloudGraph/voxel_grid_graph_builder.h>
 
#include "Common.h"
#include "DenseGraphCRF.hpp"
 
ARMARX_CONFIG_STRUCT_DEFINE_ADAPT_CONFIGURE(
    armarx::densecrf::config,
    general,
    (int,
     num_classes,
     AX_DESCRIPTION("Number of maximal object classes in an image"),
     AX_DEFAULT(20),
     AX_MIN(1),
     AX_MAX(255),
     AX_NO_REMOTE_GUI()),
    (float,
     ground_truth_prob,
     AX_DESCRIPTION("Number of maximal object classes in an image"),
     AX_DEFAULT(0.5),
     AX_MIN(0),
     AX_MAX(1),
     AX_DECIMALS(2),
     AX_STEPS(100)),
    (bool,
     use_vertex_only_graph,
     AX_DESCRIPTION("If true, the graph is not computed via a voxel grid and"
                    " therefore has no edges between the vertices. This also means the"
                    " curvature is not signed in this case."),
     AX_DEFAULT(false)),
    (bool,
     use_approximate_voxels,
     AX_DESCRIPTION("If true & UseVertexOnlyGraph is true, the downsampling"
                    " uses the ApproximateVoxelGrid for downsampling instead of the normal"
                    "VoxelGrid"),
     AX_DEFAULT(false)),
    (float,
     voxel_resolution,
     AX_DESCRIPTION("Resolution of the Voxels used as Vertices of the Graph"),
     AX_DEFAULT(0.05),
     AX_MIN(0.01),
     AX_MAX(.02),
     AX_DECIMALS(3),
     AX_STEPS(100)),
    (int,
     map_iterations,
     AX_DESCRIPTION("Number of iterations used for the map"),
     AX_DEFAULT(5),
     AX_MIN(1),
     AX_MAX(100)));
 
ARMARX_CONFIG_STRUCT_DEFINE_ADAPT_CONFIGURE(
    armarx::densecrf::config,
    output,
    (bool,
     provide_graph_pclouds,
     AX_DESCRIPTION("If true a segmentation for each timestep"
                    "is provided as result cloud"),
     AX_DEFAULT(false),
     AX_NO_REMOTE_GUI()),
    (bool,
     provide_confidence_pclouds,
     AX_DESCRIPTION("If true a confidence for each timestep"
                    "is provided as result cloud"),
     AX_DEFAULT(false),
     AX_NO_REMOTE_GUI()),
    (bool,
     colorize_confidence_pclouds,
     AX_DESCRIPTION("If true the confidence pclouds are colorized"),
     AX_DEFAULT(false),
     AX_NO_REMOTE_GUI()));
 
ARMARX_CONFIG_STRUCT_DEFINE_ADAPT_CONFIGURE(
    armarx::densecrf::config,
    edge_comp,
    (bool,
     use_xyz,
     AX_DESCRIPTION("If true xyz values are used for edge computation"),
     AX_DEFAULT(true)),
    (float,
     xyz_influence,
     AX_DESCRIPTION("Weight for the influence of position in the edge computation"),
     AX_DEFAULT(0),
     AX_MIN(-2),
     AX_MAX(2),
     AX_DECIMALS(1),
     AX_STEPS(100)),
    (bool,
     use_normals,
     AX_DESCRIPTION("If true normal values are used for edge computation"),
     AX_DEFAULT(true)),
    (float,
     normals_influence,
     AX_DESCRIPTION("Weight for the influence of normals in the edge computation"),
     AX_DEFAULT(0),
     AX_MIN(-2),
     AX_MAX(2),
     AX_DECIMALS(1),
     AX_STEPS(100)),
    (bool,
     use_rgb,
     AX_DESCRIPTION("If true RGB values are used for edge computation"),
     AX_DEFAULT(true)),
    (float,
     rgb_influence,
     AX_DESCRIPTION("Weight for the influence of color in the edge computation"),
     AX_DEFAULT(0),
     AX_MIN(-2),
     AX_MAX(2),
     AX_DECIMALS(1),
     AX_STEPS(100)),
    (bool,
     use_curvature,
     AX_DESCRIPTION("If true curvature values are used for edge computation"),
     AX_DEFAULT(true)),
    (float,
     curvature_influence,
     AX_DESCRIPTION("Weight for the influence of curvature in the edge computation"),
     AX_DEFAULT(0),
     AX_MIN(-2),
     AX_MAX(2),
     AX_DECIMALS(1),
     AX_STEPS(100)),
    (bool,
     use_time,
     AX_DESCRIPTION("If true time values are used for edge computation"),
     AX_DEFAULT(true)),
    (float,
     time_influence,
     AX_DESCRIPTION("Weight for the influence of time in the edge computation"),
     AX_DEFAULT(0),
     AX_MIN(-2),
     AX_MAX(2),
     AX_DECIMALS(1),
     AX_STEPS(100)),
    (bool,
     use_combined,
     AX_DESCRIPTION("If true the influence values are combined before they are given to DenseCRF"),
     AX_DEFAULT(true)),
    (float,
     potts_compatibilty,
     AX_DESCRIPTION("Weight factor for the Potts Compatibility"),
     AX_DEFAULT(10),
     AX_MIN(0.5),
     AX_MAX(50),
     AX_DECIMALS(1),
     AX_STEPS(200)));
 
ARMARX_CONFIG_STRUCT_DEFINE_ADAPT_CONFIGURE(armarx::densecrf::config,
                                            data,
                                            (armarx::densecrf::config::general, general),
                                            (armarx::densecrf::config::output, out),
                                            (armarx::densecrf::config::edge_comp, edge));
 
namespace armarx
{
 
    /**
     * @class DenseCRFSegmentationProcessorPropertyDefinitions
     * @brief
     */
    class DenseCRFSegmentationProcessorPropertyDefinitions :
        public visionx::PointCloudProcessorPropertyDefinitions
    {
    public:
        DenseCRFSegmentationProcessorPropertyDefinitions(std::string prefix) :
            visionx::PointCloudProcessorPropertyDefinitions(prefix)
        {
        }
    };
 
    /**
     * @defgroup Component-DenseCRFSegmentationProcessor DenseCRFSegmentationProcessor
     * @ingroup VisionX-Components
     * A description of the component DenseCRFSegmentationProcessor.
     *
     * @class DenseCRFSegmentationProcessor
     * @ingroup Component-DenseCRFSegmentationProcessor
     * @brief Brief description of class DenseCRFSegmentationProcessor.
     *
     * Detailed description of class DenseCRFSegmentationProcessor.
     */
 
    class DenseCRFSegmentationProcessor :
        virtual public DenseCRFSegmentationProcessorInterface,
        virtual public visionx::PointCloudProcessor,
        virtual public armarx::RemoteGuiComponentPluginUser
    {
    public:
        typedef boost::graph_traits<Graph>::vertex_iterator VertexIterator;
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "DenseCRFSegmentationProcessor";
        }
 
        Vector5f getCurrentEdgeWeights(const Ice::Current& current) override;
 
        void setEdgeWeights(float xyz,
                            float rgb,
                            float normals,
                            float curvature,
                            float time,
                            const Ice::Current& current) override;
 
    protected:
        /**
         * @see visionx::PointCloudProcessor::onInitPointCloudProcessor()
         */
        void onInitPointCloudProcessor() override;
 
        /**
         * @see visionx::PointCloudProcessor::onConnectPointCloudProcessor()
         */
        void onConnectPointCloudProcessor() override;
 
        /**
         * @see visionx::PointCloudProcessor::onDisconnectPointCloudProcessor()
         */
        void onDisconnectPointCloudProcessor() override;
 
        /**
         * @see visionx::PointCloudProcessor::onExitPointCloudProcessor()
         */
        void onExitPointCloudProcessor() override;
 
        /**
         * @see visionx::PointCloudProcessor::process()
         */
        void process() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
 
    private:
        template <typename TimestampMap>
        struct vertex_timestamp_unequal_filter
        {
            vertex_timestamp_unequal_filter() = default;
 
            vertex_timestamp_unequal_filter(TimestampMap tsm, double ts) : m_tsm(tsm), m_ts(ts)
            {
            }
 
            template <typename Vertex>
            bool
            operator()(const Vertex& v) const
            {
                // keep all verticers that are not the one supplied to the constructor
                double current_ts = get(m_tsm, v);
                return abs(m_ts - current_ts) > 0.00001;
            }
 
            TimestampMap m_tsm;
            double m_ts{};
        };
 
        template <typename TimestampMap>
        struct vertex_timestamp_equal_filter
        {
            vertex_timestamp_equal_filter() = default;
 
            vertex_timestamp_equal_filter(TimestampMap tsm, double ts) : m_tsm(tsm), m_ts(ts)
            {
            }
 
            template <typename Vertex>
            bool
            operator()(const Vertex& v) const
            {
                // keep all verticers that are not the one supplied to the constructor
                double current_ts = get(m_tsm, v);
                return abs(m_ts - current_ts) < 0.00001;
            }
 
            TimestampMap m_tsm;
            double m_ts;
        };
 
        double initial_time_;
        GraphPtr current_graph_ptr_;
        GraphWithTimestampPtr persistent_graph_ptr_;
 
        std::deque<Graph> graph_queue_;
        std::deque<double> timestamp_queue_;
 
        void computeGraphUsingVoxelGrid(const PointCloudT::Ptr inputCloudPtr);
 
        void computeVertexOnlyGraph(pcl::PointCloud<PointT>::Ptr input_cloud_ptr);
 
        void generateRandomClassLabels();
 
        void relabelPointCloud(pcl::PointCloud<PointT>::Ptr input_cloud_ptr);
 
        //        void computeRandomWalkerSegmentation();
 
        //        void computeEdgeWeights();
 
        void updatePersistentGraph();
 
        void addGraphToPersistentGraph(Graph& graph);
 
        void addCurrentGraphToPersistentGraph();
 
        void removeGraphFromPersistentGraph(Graph& graph);
 
        void removeCurrentGraphFromPersistentGraph();
 
        void removeTimestampFromPersistentGraph(double ts);
 
        static void copyRGBLToRGBLNormal(PointCloudT& input_cloud,
                                         PointCloudWithNormalT& output_cloud);
 
        static void copyRGBLNormalToRGBL(PointCloudWithNormalT& input_cloud,
                                         PointCloudT& output_cloud);
 
        GraphWithTimestamp retrieveGraphFromPersistentGraph(double ts);
 
        GraphWithTimestamp retrieveCurrentGraphFromPersistentGraph();
 
        template <typename FilterT>
        GraphWithTimestamp
        filterAndCopyPersistentGraph(TimestampMap tsm, FilterT filter)
        {
            ARMARX_TRACE;
            typedef boost::filtered_graph<GraphWithTimestamp, boost::keep_all, FilterT>
                FilteredGraph;
            typedef
                typename boost::graph_traits<FilteredGraph>::vertex_iterator FilteredVertexIterator;
            FilteredGraph filtered_graph =
                boost::make_filtered_graph(*persistent_graph_ptr_, boost::keep_all(), filter);
            FilteredVertexIterator vi, v_end;
            GraphWithTimestamp temp_graph;
            TimestampMap temp_tsm = boost::get(boost::vertex_timestamp_t(), temp_graph.m_graph);
            ConfidenceMap cm =
                boost::get(boost::vertex_confidence_t(), persistent_graph_ptr_->m_graph);
            ConfidenceMap temp_cm = boost::get(boost::vertex_confidence_t(), temp_graph.m_graph);
            int num_vertices = 0;
            for (boost::tie(vi, v_end) = boost::vertices(filtered_graph); vi != v_end; ++vi)
            {
                num_vertices++;
                VertexWTsId new_vertex = boost::add_vertex(temp_graph);
                boost::put(temp_tsm, new_vertex, get(tsm, *vi));
                boost::put(temp_cm, new_vertex, get(cm, *vi));
                PointWithNormalT point = filtered_graph.m_g.m_graph.m_point_cloud->points[*vi];
                temp_graph[new_vertex] = point;
            }
            //            ARMARX_INFO << "Filtered Graph has " << num_vertices << " num_vertices and "
            //                        << static_cast<int>(boost::num_vertices(temp_graph)) << " Vertices";
            return temp_graph;
        }
 
        pcl::PointCloud<pcl::PointXYZL>::ConstPtr selectRandomSeeds();
 
        Eigen::MatrixXf computeRandomUnaryEnergy(int num_points);
 
        void provideAllGraphs();
        void provideAllConfidences();
 
        WriteBufferedTripleBuffer<armarx::densecrf::config::data> config_;
        std::mutex write_mutex_;
    };
} // namespace armarx
 
#endif