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/util/CPPUtility/TripleBuffer.h>
#include <ArmarXCore/util/CPPUtility/ConfigIntrospection/create_macro.h>
 
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
// RemoteGUI
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/RemoteGuiComponentPlugin.h>
 
#include <VisionX/interface/components/DenseCRFSegmentationProcessorInterface.h>
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
#include <VisionX/libraries/PointCloudGraph/voxel_grid_graph_builder.h>
#include <VisionX/libraries/PointCloudGraph/point_cloud_graph.h>
#include <VisionX/libraries/PointCloudGraph/PointXYZRGBLNormal.h>
#include <VisionX/libraries/PointCloudGraph/common.h>
 
#include <pcl/point_types.h>
 
#include <boost/graph/filtered_graph.hpp>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "DenseGraphCRF.hpp"
#include "Common.h"
 
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_;
    };
}
 
#endif