LCCP_Segmenter.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    VisionX
 * @author     Eren Aksoy ( eren dot aksoy at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LCCP_Segmenter.h"
 
LCCPSegClass::LCCPSegClass()
{
    /// Create variables needed for preparations
    input_cloud_ptr.reset(new pcl::PointCloud<PointT>);
    input_normals_ptr.reset(new pcl::PointCloud<pcl::Normal>);
 
    sv_labeled_cloud.reset(new pcl::PointCloud<pcl::PointXYZL>);
    lccp_labeled_cloud.reset(new pcl::PointCloud<pcl::PointXYZL>);
 
    has_normals = false;
 
    ///  Default values of parameters before parsing
    // Supervoxel Stuff
    voxel_resolution = 0.0095f;
    seed_resolution = 0.06f;
    color_importance = 0.0f;
    spatial_importance = 1.0f;
    normal_importance = 4.0f;
    use_single_cam_transform = false;
    use_supervoxel_refinement = false;
 
    // LCCPSegmentation Stuff
    concavity_tolerance_threshold = 10;
    smoothness_threshold = 0.1;
    min_segment_size = 10;
    use_extended_convexity = false;
    use_sanity_criterion = false;
 
    ///  Parse Arguments needed for computation
    //Supervoxel Stuff
    normals_scale = seed_resolution / 2.0;
 
    // Segmentation Stuff
    k_factor = 0;
 
    if (use_extended_convexity)
    {
        k_factor = 1;
    }
}
 
//---------------------------------------------------------------------------------------------------------------------//
 
LCCPSegClass::~LCCPSegClass()
{
}
 
//---------------------------------------------------------------------------------------------------------------------//
 
pcl::PointCloud<pcl::PointXYZL>::Ptr&
LCCPSegClass::GetLabeledPointCloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& CloudPtr)
{
    // copy original points to the cloud
    pcl::copyPointCloud(*CloudPtr, *input_cloud_ptr);
 
    SegmentPointCloud();
 
    return lccp_labeled_cloud;
}
 
//---------------------------------------------------------------------------------------------------------------------//
 
bool
LCCPSegClass::SegmentPointCloud()
{
    /// -----------------------------------|  Main Computation  |-----------------------------------
 
    /// Preparation of Input: Supervoxel Oversegmentation
    pcl::SupervoxelClustering<PointT> super(voxel_resolution, seed_resolution);
    super.setUseSingleCameraTransform(use_single_cam_transform);
    //pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution,use_single_cam_transform);
    super.setInputCloud(input_cloud_ptr);
 
    if (has_normals)
    {
        super.setNormalCloud(input_normals_ptr);
    }
 
    super.setColorImportance(color_importance);
    super.setSpatialImportance(spatial_importance);
    super.setNormalImportance(normal_importance);
    supervoxel_clusters.clear();
 
    //PCL_INFO << " LCCP Segmenter is extracting supervoxels";
    super.extract(supervoxel_clusters);
 
    if (supervoxel_clusters.size() == 0)
    {
        //PCL_INFO << " LCCP Segmenter has no supervoxel! ";
        return false;
    }
    else
    {
        if (use_supervoxel_refinement)
        {
            //PCL_INFO << " LCCP Segmenter is refining supervoxels";
            super.refineSupervoxels(2, supervoxel_clusters);
        }
 
        //PCL_INFO<< " LCCP Segmenter found  " << supervoxel_clusters.size ()   << " supervoxels: " ;
 
        //PCL_INFO << " LCCP Segmenter is computing supervoxel adjacency";
        std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
        super.getSupervoxelAdjacency(supervoxel_adjacency);
 
        /// Get the cloud of supervoxel centroid with normals and the colored cloud with supervoxel coloring (this is used for visulization)
        pcl::PointCloud<pcl::PointNormal>::Ptr sv_centroid_normal_cloud =
            pcl::SupervoxelClustering<PointT>::makeSupervoxelNormalCloud(supervoxel_clusters);
 
        /// The Main Step: Perform LCCPSegmentation
 
        //PCL_INFO << " LCCP Segmenter is starting segmentation";
        pcl::LCCPSegmentation<PointT> lccp;
        lccp.setConcavityToleranceThreshold(concavity_tolerance_threshold);
        lccp.setSanityCheck(use_sanity_criterion);
        lccp.setSmoothnessCheck(true, voxel_resolution, seed_resolution, smoothness_threshold);
        lccp.setKFactor(k_factor);
 
        //std::cout<< " LCCP Segmenter: supervoxel_clusters size:  " << supervoxel_clusters.size ()   << " supervoxels: " << std::endl;
 
        if (min_segment_size > 0)
        {
            //PCL_INFO << " LCCP Segmenter is merging small segments";
            lccp.setMinSegmentSize(min_segment_size);
            //lccp.mergeSmallSegments(min_segment_size);
            //lccp.removeSmallSegments(min_segment_size);
        }
 
        lccp.setInputSupervoxels(supervoxel_clusters, supervoxel_adjacency);
        lccp.segment();
 
 
        //PCL_INFO << " LCCP Segmenter is interpolating voxel cloud for input cloud and relabeling";
        sv_labeled_cloud = super.getLabeledCloud();
        lccp_labeled_cloud = sv_labeled_cloud->makeShared();
        lccp.relabelCloud(*lccp_labeled_cloud);
        SuperVoxelAdjacencyList sv_adjacency_list;
        lccp.getSVAdjacencyList(sv_adjacency_list); // Needed for visualization
 
        //PCL_INFO<< " LCCP Segmenter: labeled_cloud size:  " << lccp_labeled_cloud->size() ;
 
        return true;
    }
}
 
//---------------------------------------------------------------------------------------------------------------------//
 
void
LCCPSegClass::UpdateParameters(float voxelRes,
                               float seedRes,
                               float colorImp,
                               float spatialImp,
                               float normalImp,
                               float concavityThres,
                               float smoothnessThes,
                               uint32_t minSegSize)
{
    voxel_resolution = voxelRes;
    seed_resolution = seedRes;
    color_importance = colorImp;
    spatial_importance = spatialImp;
    normal_importance = normalImp;
    concavity_tolerance_threshold = concavityThres;
    smoothness_threshold = smoothnessThes;
    min_segment_size = minSegSize;
 
    //PCL_INFO<< " LCCP Segmenter: Parameters updated! >>  minSegSize: "  << minSegSize   << "  voxelRes: "<< voxelRes << " seedRes: " << seedRes << " colorImp: " << colorImp << " spatialImp: " << spatialImp << " normalImp: " << normalImp << " concavityThres: " << concavityThres << " smoothnessThes: " << smoothnessThes;
}