DenseCRFSegmentationProcessor.cpp

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
 */
 
#include "DenseCRFSegmentationProcessor.h"
 
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/subgraph.hpp>
//#include <boost/bimap.hpp>
//#include <boost/graph/copy.hpp>
 
//#include <Eigen/Core>
//#include <Eigen/Geometry>
 
#include <algorithm>
#include <random>
 
#include <boost/smart_ptr/make_shared.hpp>
 
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include <VisionX/libraries/PointCloudGraph/PointXYZRGBLNormal.h>
 
#include "DenseCRFFeatureTerms.h"
#include "DenseGraphCRF.hpp"
#include "include/colormap/colormap.h"
 
 
using namespace armarx;
 
armarx::PropertyDefinitionsPtr
DenseCRFSegmentationProcessor::createPropertyDefinitions()
{
    armarx::PropertyDefinitionsPtr def{
        new DenseCRFSegmentationProcessorPropertyDefinitions(getConfigIdentifier())};
    def->optional(config_.getWriteBuffer(), "", "");
    return def;
}
 
void
armarx::DenseCRFSegmentationProcessor::onInitPointCloudProcessor()
{
    ARMARX_TRACE;
    std::unique_lock lock(write_mutex_);
    config_.commitWrite(); // To commit the property definitions to the buffer
    usingPointCloudProvider(getProperty<std::string>("ProviderName").getValue());
    current_graph_ptr_ = GraphPtr(new Graph);
    persistent_graph_ptr_ = GraphWithTimestampPtr(new GraphWithTimestamp);
    initial_time_ = TimeUtil::GetTime().toMilliSecondsDouble();
}
 
void
armarx::DenseCRFSegmentationProcessor::onConnectPointCloudProcessor()
{
    ARMARX_TRACE;
    //    enableResultPointClouds<PointT>();
    // add a name to provide more than one result point cloud
    enableResultPointClouds<PointT>("CRFSegmentationResult");
    enableResultPointClouds<pcl::PointXYZI>("CRFSegmentationConfidence");
    if (config_.getUpToDateReadBuffer().out.provide_graph_pclouds)
    {
        ARMARX_TRACE;
        enableResultPointClouds<PointT>("Graph0");
        enableResultPointClouds<PointT>("Graph1");
        enableResultPointClouds<PointT>("Graph2");
        enableResultPointClouds<PointT>("Graph3");
        enableResultPointClouds<PointT>("Graph4");
    }
 
    if (config_.getUpToDateReadBuffer().out.provide_confidence_pclouds)
    {
        if (config_.getUpToDateReadBuffer().out.colorize_confidence_pclouds)
        {
            ARMARX_TRACE;
            enableResultPointClouds<PointT>("Confidence0");
            enableResultPointClouds<PointT>("Confidence1");
            enableResultPointClouds<PointT>("Confidence2");
            enableResultPointClouds<PointT>("Confidence3");
            enableResultPointClouds<PointT>("Confidence4");
        }
        else
        {
            ARMARX_TRACE;
            enableResultPointClouds<pcl::PointXYZI>("Confidence0");
            enableResultPointClouds<pcl::PointXYZI>("Confidence1");
            enableResultPointClouds<pcl::PointXYZI>("Confidence2");
            enableResultPointClouds<pcl::PointXYZI>("Confidence3");
            enableResultPointClouds<pcl::PointXYZI>("Confidence4");
        }
    }
 
    {
        ARMARX_TRACE;
        createOrUpdateRemoteGuiTab(
            RemoteGui::MakeGuiConfig("Config", config_.getUpToDateReadBuffer()),
            [this](armarx::RemoteGui::TabProxy& prx)
            {
                std::unique_lock lock(write_mutex_);
                prx.getValue(config_.getWriteBuffer(), "Config");
                config_.commitWrite();
            });
    }
}
 
void
armarx::DenseCRFSegmentationProcessor::onDisconnectPointCloudProcessor()
{
}
 
void
armarx::DenseCRFSegmentationProcessor::onExitPointCloudProcessor()
{
}
 
void
armarx::DenseCRFSegmentationProcessor::process()
{
    ARMARX_TRACE;
    pcl::PointCloud<PointT>::Ptr input_cloud_ptr(new pcl::PointCloud<PointT>());
    pcl::PointCloud<PointT>::Ptr output_cloud_ptr(new pcl::PointCloud<PointT>());
 
    if (!waitForPointClouds())
    {
        ARMARX_INFO << "Timeout or error while waiting for point cloud data";
        return;
    }
    else
    {
        getPointClouds(input_cloud_ptr);
    }
    // TODO: there will be a local exception when the number of labels is bigger than the number of classes set
    //  which is hard to decipher
    std::vector<int> indices;
    input_cloud_ptr->is_dense = false;
    pcl::removeNaNFromPointCloud(*input_cloud_ptr, *input_cloud_ptr, indices);
    relabelPointCloud(input_cloud_ptr);
    ARMARX_DEBUG << "Point Cloud has " << input_cloud_ptr->size() << " Points before Downsampling";
    auto config = config_.getUpToDateReadBuffer();
    if (config.general.use_vertex_only_graph)
    {
        ARMARX_TRACE;
        computeVertexOnlyGraph(input_cloud_ptr);
    }
    else
    {
        ARMARX_TRACE;
        computeGraphUsingVoxelGrid(input_cloud_ptr);
    }
    //    if (not pre_segmented_)
    //    {
    //        computeRandomWalkerSegmentation();
    //    }
    updatePersistentGraph();
    PointCloudWithNormalT::Ptr temp_cloud_ptr = pcl::graph::point_cloud(*current_graph_ptr_);
    //    pcl::copyPointCloud(*temp_cloud_ptr, *output_cloud_ptr);
    copyRGBLNormalToRGBL(*temp_cloud_ptr, *output_cloud_ptr);
    //    generateRandomClassLabels();
    config = config_.getUpToDateReadBuffer();
    DenseGraphCRF<GraphWithTimestamp> crf(persistent_graph_ptr_, config.general.num_classes);
    crf.computeUnaryEnergyFromGraph(config.general.ground_truth_prob);
    if (config.edge.use_xyz && !config.edge.use_combined)
    {
        ARMARX_TRACE;
        TIMING_START(AddXYZFeature);
        std::vector<float> xyz_infl(3);
        std::fill(xyz_infl.begin(), xyz_infl.end(), config.edge.xyz_influence);
        crf.addPairwiseGaussianFeature<XYZFeature<GraphWithTimestamp>>(xyz_infl,
                                                                       new PottsCompatibility(10));
        TIMING_END_STREAM(AddXYZFeature, ARMARX_DEBUG);
    }
    if (config.edge.use_normals && !config.edge.use_combined)
    {
        ARMARX_TRACE;
        // TODO: use normals causes segfault
        TIMING_START(AddNormalFeature);
        std::vector<float> normals_infl(3);
        std::fill(normals_infl.begin(), normals_infl.end(), config.edge.normals_influence);
        crf.addPairwiseGaussianFeature<NormalFeature<GraphWithTimestamp>>(
            normals_infl, new PottsCompatibility(10));
        TIMING_END_STREAM(AddNormalFeature, ARMARX_DEBUG);
    }
    if (config.edge.use_curvature && !config.edge.use_combined)
    {
        ARMARX_TRACE;
        TIMING_START(AddCurvatureFeature);
        std::vector<float> curv_infl(1);
        std::fill(curv_infl.begin(), curv_infl.end(), config.edge.curvature_influence);
        crf.addPairwiseGaussianFeature<CurvatureFeature<GraphWithTimestamp>>(
            curv_infl, new PottsCompatibility(10));
        TIMING_END_STREAM(AddCurvatureFeature, ARMARX_DEBUG);
    }
    if (config.edge.use_rgb && !config.edge.use_combined)
    {
        ARMARX_TRACE;
        TIMING_START(AddRGBFeature);
        std::vector<float> rgb_infl(3);
        std::fill(rgb_infl.begin(), rgb_infl.end(), config.edge.rgb_influence);
        crf.addPairwiseGaussianFeature<RGBFeature<GraphWithTimestamp>>(rgb_infl,
                                                                       new PottsCompatibility(10));
        TIMING_END_STREAM(AddRGBFeature, ARMARX_DEBUG);
    }
    if (config.edge.use_time && !config.edge.use_combined)
    {
        ARMARX_TRACE;
        TIMING_START(AddTimeFeature);
        std::vector<float> time_infl(1);
        std::fill(time_infl.begin(), time_infl.end(), config.edge.time_influence);
        crf.addPairwiseGaussianFeature<TimeFeature>(time_infl, new PottsCompatibility(1));
        TIMING_END_STREAM(AddTimeFeature, ARMARX_DEBUG);
    }
    if (config.edge.use_combined)
    {
        ARMARX_TRACE;
        TIMING_START(AddCombinedFeature);
        int num_feat = 0;
        num_feat += config.edge.use_rgb ? 3 : 0;
        num_feat += config.edge.use_normals ? 3 : 0;
        num_feat += config.edge.use_xyz ? 3 : 0;
        num_feat += config.edge.use_time ? 1 : 0;
        num_feat += config.edge.use_curvature ? 1 : 0;
        std::vector<float> comb_infl(num_feat);
        int j;
        for (int i = 0; i < 3; i++)
        {
            j = 0;
            if (config.edge.use_rgb)
            {
                comb_infl[i + j] = config.edge.rgb_influence;
                j += 3;
            }
            if (config.edge.use_normals)
            {
                comb_infl[i + j] = config.edge.normals_influence;
                j += 3;
            }
            if (config.edge.use_xyz)
            {
                comb_infl[i + j] = config.edge.xyz_influence;
                j += 3;
            }
        }
        if (config.edge.use_curvature)
        {
            comb_infl[j] = config.edge.curvature_influence;
            j++;
        }
        if (config.edge.use_time)
        {
            comb_infl[j] = config.edge.time_influence;
        }
        crf.addCombinedGaussianFeature(comb_infl,
                                       config.edge.use_rgb,
                                       config.edge.use_normals,
                                       config.edge.use_xyz,
                                       config.edge.use_curvature,
                                       config.edge.use_time,
                                       new PottsCompatibility(config.edge.potts_compatibilty));
        TIMING_END_STREAM(AddCombinedFeature, ARMARX_DEBUG);
    }
 
 
    //    VectorXs map = crf.map(map_iterations_);
    ARMARX_TRACE;
    TIMING_START(MAP);
    Eigen::VectorXf confidence = crf.map_and_confidence(config.general.map_iterations);
    TIMING_END_STREAM(MAP, ARMARX_DEBUG);
    if (config.out.provide_graph_pclouds)
    {
        provideAllGraphs();
    }
    if (config.out.provide_confidence_pclouds)
    {
        provideAllConfidences();
    }
    output_cloud_ptr->points.clear();
    GraphWithTimestamp outputGraph = retrieveCurrentGraphFromPersistentGraph();
    //    ConfidenceMap cm = boost::get(boost::vertex_confidence_t(), outputGraph.m_graph);
 
 
    temp_cloud_ptr = pcl::graph::point_cloud(outputGraph);
    //    pcl::copyPointCloud(*temp_cloud_ptr, *output_cloud_ptr);
    copyRGBLNormalToRGBL(*temp_cloud_ptr, *output_cloud_ptr);
    // FIXME: providePointClouds segfaults when called with pcl::pointXYZRGB
    outputGraph = retrieveGraphFromPersistentGraph(timestamp_queue_.back());
    ConfidenceMap cm = boost::get(boost::vertex_confidence_t(), outputGraph.m_graph);
    temp_cloud_ptr = pcl::graph::point_cloud(outputGraph);
    pcl::PointCloud<pcl::PointXYZI>::Ptr confidence_cloud_ptr(
        new pcl::PointCloud<pcl::PointXYZI>());
    pcl::copyPointCloud(*temp_cloud_ptr, *confidence_cloud_ptr);
    for (long unsigned int i = 0; i < confidence_cloud_ptr->points.size(); i++)
    {
        //        float angle = confidence[i] * M_PI;
        //        float red = 255.0 * sin(angle);
        //        float green = 255.0 * sin(angle + 2 * M_PI / 3.); // + 60°
        //        float blue = 255.0 * sin(angle + 4 * M_PI / 3.); // + 120°
        float conf = cm[i];
        //        colormap::Color c = jet.getColor(conf);
        //        int rgb = int(conf*2147483647);
        //        uint8_t r = (rgb >> 16) & 0x0000ff;
        //        uint8_t g = (rgb >> 8)  & 0x0000ff;
        //        uint8_t b = (rgb)     & 0x0000ff;
 
 
        //        confidence_cloud_ptr->points[i].r = c.r * 255.0;
        //        confidence_cloud_ptr->points[i].g = c.g * 255.0;
        //        confidence_cloud_ptr->points[i].b = c.b * 255.0;
        confidence_cloud_ptr->points[i].intensity = conf;
    }
    ARMARX_TRACE;
    output_cloud_ptr->header = input_cloud_ptr->header;
    provideResultPointClouds<decltype(output_cloud_ptr)>(output_cloud_ptr, "CRFSegmentationResult");
    provideResultPointClouds<decltype(confidence_cloud_ptr)>(confidence_cloud_ptr,
                                                             "CRFSegmentationConfidence");
    pcl::PointXYZRGBA i;
}
 
void
DenseCRFSegmentationProcessor::computeGraphUsingVoxelGrid(const PointCloudT::Ptr inputCloudPtr)
{
    const auto& config = config_.getUpToDateReadBuffer();
    double r = config.general.voxel_resolution;
    pcl::graph::VoxelGridGraphBuilder<PointT, Graph> graph_builder(r);
    graph_builder.setInputCloud(inputCloudPtr);
    TIMING_START(ComputeGraph);
    graph_builder.compute(*current_graph_ptr_);
    TIMING_END_STREAM(ComputeGraph, ARMARX_DEBUG);
    ARMARX_DEBUG << "Graph has " << static_cast<int>(boost::num_vertices(*current_graph_ptr_))
                 << " Vertices and " << static_cast<int>(boost::num_edges(*current_graph_ptr_))
                 << " Edges";
    if (config.edge.use_curvature || config.edge.use_normals)
    {
        TIMING_START(ComputeNormalsAndCurvatures);
        pcl::graph::computeNormalsAndCurvatures(*current_graph_ptr_);
        TIMING_END_STREAM(ComputeNormalsAndCurvatures, ARMARX_DEBUG);
        TIMING_START(ComputeSignedCurvatures);
        pcl::graph::computeSignedCurvatures(*current_graph_ptr_);
        TIMING_END_STREAM(ComputeSignedCurvatures, ARMARX_DEBUG);
    }
}
 
Eigen::MatrixXf
DenseCRFSegmentationProcessor::computeRandomUnaryEnergy(int num_points)
{
    const auto& config = config_.getUpToDateReadBuffer();
    Eigen::MatrixXf energy(config.general.num_classes, num_points);
    const double u_energy = -log(1.0 / config.general.num_classes);
    const double n_energy =
        -log((1.0 - config.general.ground_truth_prob) / (config.general.num_classes - 1));
    const double p_energy = -log(config.general.ground_truth_prob);
    energy.fill(u_energy);
    std::random_device dev;
    std::mt19937 rng(dev());
    std::uniform_int_distribution<std::mt19937::result_type> dist(0,
                                                                  config.general.num_classes - 1);
    for (int k = 0; k < num_points; k++)
    {
        // Set the energy
        int r = dist(rng);
        if (r >= 0)
        {
            energy.col(k).fill(n_energy);
            energy(r, k) = p_energy;
        }
    }
    return energy;
}
 
void
DenseCRFSegmentationProcessor::generateRandomClassLabels()
{
 
    VertexIterator vi, v_end;
    std::random_device dev;
    std::mt19937 rng(dev());
    std::uniform_int_distribution<std::mt19937::result_type> dist(
        0, config_.getUpToDateReadBuffer().general.num_classes - 1);
    for (boost::tie(vi, v_end) = boost::vertices(*current_graph_ptr_); vi != v_end; ++vi)
    {
        int r = dist(rng);
        (*current_graph_ptr_)[*vi].label = r;
    }
}
 
//void DenseCRFSegmentationProcessor::computeRandomWalkerSegmentation()
//{
//    computeEdgeWeights();
//    pcl::segmentation::RandomWalkerSegmentation<PointT> rws;
//    rws.setInputGraph(current_graph_ptr_);
//    rws.setSeeds(selectRandomSeeds());
//    std::vector<pcl::PointIndices> clusters;
//    rws.segment(clusters);
//    PointCloudWithNormalT::Ptr tempCloud = pcl::graph::point_cloud(*current_graph_ptr_);
//    VertexIterator vi, v_end;
//    int lbl = 0;
//    for (auto clstr_ptr = clusters.begin(); clstr_ptr != clusters.end(); clstr_ptr++, lbl++)
//    {
//        for (auto pt_ind_ptr = clstr_ptr->indices.begin(); pt_ind_ptr != clstr_ptr->indices.end(); pt_ind_ptr++)
//        {
//            (*current_graph_ptr_)[static_cast<Graph::vertex_descriptor>(*pt_ind_ptr)].label = lbl;
//        }
//    }
//}
 
pcl::PointCloud<pcl::PointXYZL>::ConstPtr
DenseCRFSegmentationProcessor::selectRandomSeeds()
{
    auto num_classes = config_.getUpToDateReadBuffer().general.num_classes;
    VertexIterator vi, v_end;
    std::vector<Graph::vertex_descriptor> v_out;
    boost::tie(vi, v_end) = boost::vertices(*current_graph_ptr_);
    // select num_classes_ random samples from the vertices as seeds
    std::sample(vi,
                v_end,
                std::back_inserter(v_out),
                num_classes - 1,
                std::mt19937{std::random_device{}()});
 
    PointCloudWithNormalT::Ptr tempCloudPtr(new PointCloudWithNormalT(num_classes, 1));
    int j = 0;
    for (auto i = v_out.begin(); i != v_out.end(); i++, j++)
    {
        tempCloudPtr->points[j] = (*current_graph_ptr_)[*i];
        // each seed gets an individiual label
        tempCloudPtr->points[j].label = j;
    }
 
    pcl::PointCloud<pcl::PointXYZL>::Ptr outCloudPtr(new pcl::PointCloud<pcl::PointXYZL>());
    pcl::copyPointCloud(*tempCloudPtr, *outCloudPtr);
    return outCloudPtr;
}
 
//void DenseCRFSegmentationProcessor::computeEdgeWeights()
//{
//    using namespace pcl::graph;
//    typedef EdgeWeightComputer<Graph> EWC;
//    EWC computer;
//    if (use_xyz_)
//    {
//        float influence = xyz_influence_;
//        float multiplier = 1.0;
//        computer.addTerm<terms::XYZ>(influence, multiplier, EWC::NORMALIZATION_LOCAL);
//    }
//    if (use_normals_)
//    {
//        float influence = normals_influence_;
//        float multiplier = 1.0;
//        computer.addTerm<terms::Normal>(influence, multiplier);
//    }
//    if (use_curvature_)
//    {
//        float influence = curvature_influence_;
//        float multiplier = 1.0;
//        computer.addTerm<terms::Curvature>(influence, multiplier);
//    }
//    if (use_rgb_)
//    {
//        float influence = rgb_influence_;
//        float multiplier = 1.0;
//        computer.addTerm<terms::RGB>(influence, multiplier, EWC::NORMALIZATION_GLOBAL);
//    }
//    computer.setSmallWeightThreshold(1e-5);
//    computer.setSmallWeightPolicy(EWC::SMALL_WEIGHT_COERCE_TO_THRESHOLD);
//    computer.compute(*current_graph_ptr_);
//}
 
void
DenseCRFSegmentationProcessor::updatePersistentGraph()
{
    ARMARX_TRACE;
    TIMING_START(UpdateRootGraph)
    double current_ts =
        static_cast<double>(current_graph_ptr_->m_graph.m_point_cloud->header.stamp / 1000.0) -
        initial_time_;
    timestamp_queue_.push_back(current_ts);
    //    graph_queue_.push_back(*current_graph_ptr_);
    if (timestamp_queue_.size() > 5)
    {
        ARMARX_TRACE;
        double tsToRemove = timestamp_queue_.front();
        timestamp_queue_.pop_front();
        removeTimestampFromPersistentGraph(tsToRemove);
    }
    addCurrentGraphToPersistentGraph();
    ARMARX_DEBUG << "Root Graph consisting of " << timestamp_queue_.size() << " Subgraphs has "
                 << static_cast<int>(boost::num_vertices(*persistent_graph_ptr_)) << " Vertices";
    TIMING_END_STREAM(UpdateRootGraph, ARMARX_DEBUG)
}
 
void
DenseCRFSegmentationProcessor::addGraphToPersistentGraph(Graph& graph)
{
    TimestampMap tsm = boost::get(boost::vertex_timestamp_t(), persistent_graph_ptr_->m_graph);
    ConfidenceMap cm = boost::get(boost::vertex_confidence_t(), persistent_graph_ptr_->m_graph);
    VertexIterator vi, v_end;
    double current_ts =
        static_cast<double>(graph.m_graph.m_point_cloud->header.stamp / 1000.0) - initial_time_;
    for (boost::tie(vi, v_end) = boost::vertices(graph); vi != v_end; ++vi)
    {
        VertexWTsId new_vertex = boost::add_vertex(*persistent_graph_ptr_);
        boost::put(tsm, new_vertex, current_ts);
        // value of -1.0 signals it has not been set
        boost::put(cm, new_vertex, -1.0);
        PointWithNormalT point = graph.m_graph.m_point_cloud->points[*vi];
        (*persistent_graph_ptr_)[new_vertex] = point;
    }
}
 
void
DenseCRFSegmentationProcessor::addCurrentGraphToPersistentGraph()
{
    ARMARX_TRACE;
    addGraphToPersistentGraph(*current_graph_ptr_);
}
 
void
DenseCRFSegmentationProcessor::removeGraphFromPersistentGraph(Graph& graph)
{
    ARMARX_TRACE;
    double graph_ts =
        static_cast<double>(graph.m_graph.m_point_cloud->header.stamp / 1000.0) - initial_time_;
    removeTimestampFromPersistentGraph(graph_ts);
}
 
void
DenseCRFSegmentationProcessor::removeCurrentGraphFromPersistentGraph()
{
    ARMARX_TRACE;
    removeGraphFromPersistentGraph(*current_graph_ptr_);
}
 
void
DenseCRFSegmentationProcessor::removeTimestampFromPersistentGraph(double ts)
{
    ARMARX_TRACE;
    TimestampMap tsm = boost::get(boost::vertex_timestamp_t(), persistent_graph_ptr_->m_graph);
    typedef vertex_timestamp_unequal_filter<TimestampMap> FilterT;
    FilterT filter(tsm, ts);
    GraphWithTimestamp filtered_graph = filterAndCopyPersistentGraph<FilterT>(tsm, filter);
    GraphWithTimestampPtr temp_graph_ptr = boost::make_shared<GraphWithTimestamp>(filtered_graph);
    persistent_graph_ptr_.swap(temp_graph_ptr);
}
 
GraphWithTimestamp
DenseCRFSegmentationProcessor::retrieveGraphFromPersistentGraph(double ts)
{
    ARMARX_TRACE;
    TimestampMap tsm = boost::get(boost::vertex_timestamp_t(), persistent_graph_ptr_->m_graph);
    typedef vertex_timestamp_equal_filter<TimestampMap> FilterT;
    FilterT filter(tsm, ts);
    GraphWithTimestamp filtered_graph = filterAndCopyPersistentGraph<FilterT>(tsm, filter);
    return filtered_graph;
}
 
GraphWithTimestamp
DenseCRFSegmentationProcessor::retrieveCurrentGraphFromPersistentGraph()
{
    ARMARX_TRACE;
    double current_ts =
        static_cast<double>(current_graph_ptr_->m_graph.m_point_cloud->header.stamp / 1000.0) -
        initial_time_;
    return retrieveGraphFromPersistentGraph(current_ts);
}
 
void
DenseCRFSegmentationProcessor::provideAllGraphs()
{
    ARMARX_TRACE;
    for (unsigned int i = 0; i < timestamp_queue_.size(); i++)
    {
        double ts = timestamp_queue_.at(i);
        GraphWithTimestamp graph = retrieveGraphFromPersistentGraph(ts);
        PointCloudT::Ptr output_cloud_ptr(new PointCloudT());
        PointCloudWithNormalT::Ptr temp_cloud_ptr(new PointCloudWithNormalT());
        temp_cloud_ptr = pcl::graph::point_cloud(graph);
        //        pcl::graph::indices (Subgraph& g)
        //        pcl::copyPointCloud(*temp_cloud_ptr, *output_cloud_ptr);
        // FIXME: providePointClouds segfaults when called with pcl::pointXYZRGB
        copyRGBLNormalToRGBL(*temp_cloud_ptr, *output_cloud_ptr);
        provideResultPointClouds(output_cloud_ptr, "Graph" + std::to_string(i));
    }
}
 
void
DenseCRFSegmentationProcessor::computeVertexOnlyGraph(PointCloudT::Ptr input_cloud_ptr)
{
    ARMARX_TRACE;
    const auto& config = config_.getUpToDateReadBuffer();
    TIMING_START(VoxelDownsample);
    pcl::PointCloud<PointT>::Ptr temp_cloud_ptr(new pcl::PointCloud<PointT>());
    ARMARX_DEBUG << input_cloud_ptr->size() << "before downsampling";
    // TODO: Voxel downsampling produces a cloud with a=0 for every point; Fix this
    if (config.general.use_approximate_voxels)
    {
        ARMARX_TRACE;
        pcl::ApproximateVoxelGrid<PointT> vg;
        vg.setInputCloud(input_cloud_ptr);
        vg.setLeafSize(config.general.voxel_resolution * 100,
                       config.general.voxel_resolution * 100,
                       config.general.voxel_resolution * 100);
        vg.filter(*temp_cloud_ptr);
        temp_cloud_ptr.swap(input_cloud_ptr);
    }
    else
    {
        ARMARX_TRACE;
        pcl::VoxelGrid<PointT> vg;
        vg.setInputCloud(input_cloud_ptr);
        vg.setLeafSize(config.general.voxel_resolution * 100,
                       config.general.voxel_resolution * 100,
                       config.general.voxel_resolution * 100);
        vg.filter(*temp_cloud_ptr);
        temp_cloud_ptr.swap(input_cloud_ptr);
    }
    ARMARX_TRACE;
    ARMARX_DEBUG << input_cloud_ptr->size() << "after downsampling";
    TIMING_END_STREAM(VoxelDownsample, ARMARX_DEBUG);
    TIMING_START(NormalComputation);
    pcl::NormalEstimation<PointT, PointWithNormalT> ne;
    ne.setInputCloud(input_cloud_ptr);
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
    ne.setSearchMethod(tree);
    PointCloudWithNormalT::Ptr cloud_normals(new PointCloudWithNormalT());
    //    pcl::copyPointCloud(*input_cloud_ptr, *cloud_normals);
    copyRGBLToRGBLNormal(*input_cloud_ptr, *cloud_normals);
    // Use all neighbors in a sphere of radius 3cm
    ne.setRadiusSearch(100 * 5 * config.general.voxel_resolution);
    ARMARX_TRACE;
    // Compute the features
    ne.compute(*cloud_normals);
    TIMING_END_STREAM(NormalComputation, ARMARX_DEBUG);
    GraphPtr temp_graph_ptr = boost::make_shared<Graph>(Graph(cloud_normals));
    current_graph_ptr_.swap(temp_graph_ptr);
    ARMARX_DEBUG << "Graph has " << static_cast<int>(boost::num_vertices(*current_graph_ptr_))
                 << " Vertices and " << static_cast<int>(boost::num_edges(*current_graph_ptr_))
                 << " Edges";
}
 
void
DenseCRFSegmentationProcessor::copyRGBLToRGBLNormal(PointCloudT& input_cloud,
                                                    PointCloudWithNormalT& output_cloud)
{
    ARMARX_TRACE;
    output_cloud.header = input_cloud.header;
    output_cloud.width = input_cloud.width;
    output_cloud.height = input_cloud.height;
    output_cloud.is_dense = input_cloud.is_dense;
    output_cloud.sensor_orientation_ = input_cloud.sensor_orientation_;
    output_cloud.sensor_origin_ = input_cloud.sensor_origin_;
    output_cloud.points.resize(input_cloud.points.size());
 
    if (input_cloud.points.size() == 0)
    {
        ARMARX_TRACE;
        return;
    }
    ARMARX_TRACE;
    for (size_t i = 0; i < output_cloud.size(); i++)
    {
        output_cloud.points[i].x = input_cloud.points[i].x;
        output_cloud.points[i].y = input_cloud.points[i].y;
        output_cloud.points[i].z = input_cloud.points[i].z;
        output_cloud.points[i].r = input_cloud.points[i].r;
        output_cloud.points[i].g = input_cloud.points[i].g;
        output_cloud.points[i].b = input_cloud.points[i].b;
        output_cloud.points[i].a = uint8_t(255);
        output_cloud.points[i].label = input_cloud.points[i].label;
        output_cloud.points[i].normal_x = 0;
        output_cloud.points[i].normal_y = 0;
        output_cloud.points[i].normal_z = 0;
        output_cloud.points[i].curvature = 0;
    }
}
 
void
DenseCRFSegmentationProcessor::copyRGBLNormalToRGBL(PointCloudWithNormalT& input_cloud,
                                                    PointCloudT& output_cloud)
{
    ARMARX_TRACE;
    output_cloud.header = input_cloud.header;
    output_cloud.width = input_cloud.width;
    output_cloud.height = input_cloud.height;
    output_cloud.is_dense = input_cloud.is_dense;
    output_cloud.sensor_orientation_ = input_cloud.sensor_orientation_;
    output_cloud.sensor_origin_ = input_cloud.sensor_origin_;
    output_cloud.points.resize(input_cloud.points.size());
 
    if (input_cloud.points.size() == 0)
    {
        return;
    }
    ARMARX_TRACE;
    for (size_t i = 0; i < output_cloud.size(); i++)
    {
        output_cloud.points[i].x = input_cloud.points[i].x;
        output_cloud.points[i].y = input_cloud.points[i].y;
        output_cloud.points[i].z = input_cloud.points[i].z;
        output_cloud.points[i].r = input_cloud.points[i].r;
        output_cloud.points[i].g = input_cloud.points[i].g;
        output_cloud.points[i].b = input_cloud.points[i].b;
        output_cloud.points[i].a = uint8_t(255);
        output_cloud.points[i].label = input_cloud.points[i].label;
    }
}
 
void
DenseCRFSegmentationProcessor::relabelPointCloud(pcl::PointCloud<PointT>::Ptr input_cloud_ptr)
{
    ARMARX_TRACE;
    std::map<uint32_t, uint32_t> labelmap;
    uint32_t highest_new_label = 1;
    for (auto& point : input_cloud_ptr->points)
    {
        if (labelmap.find(point.label) != labelmap.end())
        {
            point.label = labelmap.at(point.label);
        }
        else
        {
            labelmap.insert(std::make_pair(point.label, highest_new_label));
            point.label = highest_new_label;
            highest_new_label++;
        }
    }
}
 
void
DenseCRFSegmentationProcessor::provideAllConfidences()
{
    ARMARX_TRACE;
    colormap::MATLAB::Jet jet;
    bool colorize = config_.getUpToDateReadBuffer().out.colorize_confidence_pclouds;
    for (unsigned int i = 0; i < timestamp_queue_.size(); i++)
    {
        double ts = timestamp_queue_.at(i);
        GraphWithTimestamp graph = retrieveGraphFromPersistentGraph(ts);
        ConfidenceMap cm = boost::get(boost::vertex_confidence_t(), graph.m_graph);
        PointCloudWithNormalT::Ptr temp_cloud_ptr(new PointCloudWithNormalT());
        temp_cloud_ptr = pcl::graph::point_cloud(graph);
        //        pcl::graph::indices (Subgraph& g)
        //        pcl::copyPointCloud(*temp_cloud_ptr, *output_cloud_ptr);
        // FIXME: providePointClouds segfaults when called with pcl::pointXYZRGB
        if (colorize)
        {
            PointCloudT::Ptr output_cloud_ptr(new PointCloudT());
            copyRGBLNormalToRGBL(*temp_cloud_ptr, *output_cloud_ptr);
            for (long unsigned int j = 0; j < output_cloud_ptr->points.size(); j++)
            {
                float conf = cm[j];
                colormap::Color c = jet.getColor(conf);
                output_cloud_ptr->points[j].r = c.r * 255.0;
                output_cloud_ptr->points[j].g = c.g * 255.0;
                output_cloud_ptr->points[j].b = c.b * 255.0;
            }
            provideResultPointClouds<decltype(output_cloud_ptr)>(output_cloud_ptr,
                                                                 "Confidence" + std::to_string(i));
        }
        else
        {
            pcl::PointCloud<pcl::PointXYZI>::Ptr confidence_cloud_ptr(
                new pcl::PointCloud<pcl::PointXYZI>());
            pcl::copyPointCloud(*temp_cloud_ptr, *confidence_cloud_ptr);
            for (long unsigned int j = 0; j < confidence_cloud_ptr->points.size(); j++)
            {
                float conf = cm[j];
                confidence_cloud_ptr->points[j].intensity = conf;
            }
            provideResultPointClouds<decltype(confidence_cloud_ptr)>(
                confidence_cloud_ptr, "Confidence" + std::to_string(i));
        }
    }
}
 
Vector5f
DenseCRFSegmentationProcessor::getCurrentEdgeWeights(const Ice::Current& current)
{
    const auto& config = config_.getUpToDateReadBuffer().edge;
    Vector5f vec{config.xyz_influence,
                 config.rgb_influence,
                 config.normals_influence,
                 config.curvature_influence,
                 config.time_influence};
    return vec;
}
 
void
DenseCRFSegmentationProcessor::setEdgeWeights(float xyz,
                                              float rgb,
                                              float normals,
                                              float curvature,
                                              float time,
                                              const Ice::Current& current)
{
    // TODO: use mutex
    std::unique_lock lock(write_mutex_);
    auto& config = config_.getWriteBuffer().edge;
    config.xyz_influence = xyz;
    config.rgb_influence = rgb;
    config.normals_influence = normals;
    config.curvature_influence = curvature;
    config.time_influence = time;
    config_.commitWrite();
}