PrimitiveExtractionParameterTuning.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    VisionX::ArmarXObjects::PrimitiveExtractionParameterTuning
 * @author     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PrimitiveExtractionParameterTuning.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <pcl/octree/octree.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
using namespace armarx;
 
void
PrimitiveExtractionParameterTuning::onInitPointCloudProcessor()
{
    primitivesProviderName = getProperty<std::string>("PrimitivesProviderName").getValue();
    usingPointCloudProvider(primitivesProviderName);
 
    usingProxy(getProperty<std::string>("PointCloudProviderName").getValue());
    usingProxy(getProperty<std::string>("PointCloudSegmenterName").getValue());
    usingProxy(getProperty<std::string>("PrimitiveExtractorName").getValue());
 
    sourcePointCloudFilename = getProperty<std::string>("SourcePointCloud").getValue();
    referencePointCloudFilename = getProperty<std::string>("GroundTruthPointCloud").getValue();
    exportDirectory = getProperty<std::string>("ExportDirectory").getValue();
 
    // Load reference point cloud
    if (!ArmarXDataPath::getAbsolutePath(sourcePointCloudFilename, sourcePointCloudFilename) ||
        !ArmarXDataPath::getAbsolutePath(referencePointCloudFilename,
                                         referencePointCloudFilename) ||
        !ArmarXDataPath::getAbsolutePath(exportDirectory, exportDirectory))
    {
        ARMARX_ERROR << "Could not find point cloud files in ArmarXDataPath";
        return;
    }
 
    referencePointCloud.reset(new pcl::PointCloud<pcl::PointXYZRGBL>());
    if (pcl::io::loadPCDFile(referencePointCloudFilename.c_str(), *referencePointCloud) == -1)
    {
        ARMARX_WARNING << "Unable to load point cloud from file "
                       << getProperty<std::string>("GroundTruthPointCloud").getValue();
        return;
    }
 
    // Initialize default parameters
    extractionPrm.minSegmentSize = 100;
    extractionPrm.maxSegmentSize = 250000000;
    extractionPrm.euclideanClusteringTolerance = 30;
    extractionPrm.outlierThreshold = 20;
 
    extractionPrm.planeMaxIterations = 100;
    extractionPrm.planeDistanceThreshold = 40;
    extractionPrm.planeNormalDistance = 10;
 
    extractionPrm.cylinderMaxIterations = 100;
    extractionPrm.cylinderDistanceThreshold = 10;
    extractionPrm.cylinderRadiusLimit = 10;
 
    extractionPrm.sphereMaxIterations = 100;
    extractionPrm.sphereDistanceThreshold = 90;
    extractionPrm.sphereNormalDistance = 10;
    extractionPrm.circularDistanceThreshold = 10;
 
    segmentationPrm.minSegmentSize = 5;
    segmentationPrm.voxelResolution = 20;
    segmentationPrm.seedResolution = 70;
    segmentationPrm.colorImportance = 0;
    segmentationPrm.spatialImportance = 1;
    segmentationPrm.normalImportance = 4;
    segmentationPrm.concavityThreshold = 10;
    segmentationPrm.smoothnessThreshold = 0.1;
 
    bestFitness = -std::numeric_limits<float>::infinity();
 
    // Initialize bounds
    bounds_segmentation_minSegmentSize = Eigen::Vector2f(100, 1000);
    bounds_segmentation_maxSegmentSize = Eigen::Vector2f(250000000, 250000000);
    bounds_segmentation_euclideanClusteringTolerance = Eigen::Vector2f(1, 100);
    bounds_segmentation_outlierThreshold = Eigen::Vector2f(1, 100);
    bounds_segmentation_planeMaxIterations = Eigen::Vector2f(100, 100);
    bounds_segmentation_planeDistanceThreshold = Eigen::Vector2f(1e-6, 100);
    bounds_segmentation_planeNormalDistance = Eigen::Vector2f(1e-6, 100);
    bounds_segmentation_cylinderMaxIterations = Eigen::Vector2f(100, 100);
    bounds_segmentation_cylinderDistanceThreshold = Eigen::Vector2f(1e-6, 100);
    bounds_segmentation_cylinderRadiusLimit = Eigen::Vector2f(1e-6, 100);
    bounds_segmentation_sphereMaxIterations = Eigen::Vector2f(100, 100);
    bounds_segmentation_sphereDistanceThreshold = Eigen::Vector2f(1e-6, 100);
    bounds_segmentation_sphereNormalDistance = Eigen::Vector2f(1e-6, 100);
    bounds_segmentation_circularDistanceThreshold = Eigen::Vector2f(1, 100);
    bounds_primitives_minSegmentSize = Eigen::Vector2f(1, 100);
    bounds_primitives_voxelResolution = Eigen::Vector2f(1, 100);
    bounds_primitives_seedResolution = Eigen::Vector2f(1, 100);
    bounds_primitives_colorImportance = Eigen::Vector2f(0, 0);
    bounds_primitives_spatialImportance = Eigen::Vector2f(1e-6, 100);
    bounds_primitives_normalImportance = Eigen::Vector2f(1e-6, 100);
    bounds_primitives_concavityThreshold = Eigen::Vector2f(1e-6, 100);
    bounds_primitives_smoothnessThreshold = Eigen::Vector2f(1e-6, 100);
}
 
void
PrimitiveExtractionParameterTuning::onConnectPointCloudProcessor()
{
    pointCloudProvider = getProxy<visionx::FakePointCloudProviderInterfacePrx>(
        getProperty<std::string>("PointCloudProviderName").getValue());
    if (!pointCloudProvider)
    {
        ARMARX_ERROR << "Could not obtain point cloud provider proxy";
        return;
    }
 
    pointCloudSegmenter = getProxy<visionx::PointCloudSegmenterInterfacePrx>(
        getProperty<std::string>("PointCloudSegmenterName").getValue());
    if (!pointCloudSegmenter)
    {
        ARMARX_ERROR << "Could not obtain point cloud segmenter proxy";
        return;
    }
 
    primitiveExtractor = getProxy<visionx::PrimitiveMapperInterfacePrx>(
        getProperty<std::string>("PrimitiveExtractorName").getValue());
    if (!primitiveExtractor)
    {
        ARMARX_ERROR << "Could not obtain primitive extractor";
        return;
    }
 
    iteration = 1;
 
    primitiveExtractor->setParameters(extractionPrm);
    pointCloudSegmenter->setLccpParameters(segmentationPrm);
 
    pointCloudProvider->setPointCloudFilename(sourcePointCloudFilename);
    pointCloudProvider->begin_startCaptureForNumFrames(1);
}
 
void
PrimitiveExtractionParameterTuning::onDisconnectPointCloudProcessor()
{
}
 
void
PrimitiveExtractionParameterTuning::onExitPointCloudProcessor()
{
}
 
void
PrimitiveExtractionParameterTuning::process()
{
    if (!waitForPointClouds(primitivesProviderName, 10000))
    {
        return;
    }
 
    pcl::PointCloud<pcl::PointXYZL>::Ptr primitives(new pcl::PointCloud<pcl::PointXYZL>());
    if (!getPointClouds(primitivesProviderName, primitives))
    {
        ARMARX_WARNING << "Unable to get point cloud data.";
        return;
    }
 
    float fitness = compareLabeledPointClouds(primitives, referencePointCloud);
    ARMARX_INFO << "Processed point cloud #" << iteration << " received (fitness = " << fitness
                << ")";
 
    if (fitness > bestFitness)
    {
        ARMARX_INFO << "New best parameter set found";
        bestSegmentationPrm = segmentationPrm;
        bestExtractionPrm = extractionPrm;
        bestFitness = fitness;
 
        exportBestSetup(exportDirectory, primitives, bestSegmentationPrm, bestExtractionPrm);
    }
    else
    {
        ARMARX_INFO << "Current parameter set not better";
    }
 
    iteration++;
    generateNewParameterSet();
 
    primitiveExtractor->setParameters(extractionPrm);
    pointCloudSegmenter->setLccpParameters(segmentationPrm);
    pointCloudProvider->begin_startCaptureForNumFrames(1);
}
 
armarx::PropertyDefinitionsPtr
PrimitiveExtractionParameterTuning::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new PrimitiveExtractionParameterTuningPropertyDefinitions(getConfigIdentifier()));
}
 
float
PrimitiveExtractionParameterTuning::compareLabeledPointClouds(
    const pcl::PointCloud<pcl::PointXYZL>::Ptr& labeled,
    const pcl::PointCloud<pcl::PointXYZRGBL>::Ptr& reference)
{
    ARMARX_INFO << labeled->size() << " points in segmentation point cloud";
    ARMARX_INFO << reference->size() << " points in ground truth point cloud";
 
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZL> octree(50);
    octree.setInputCloud(labeled);
    octree.addPointsFromInputCloud();
 
    std::map<unsigned int, unsigned int> labelMap;
 
    unsigned int num_covered = 0;
    for (auto& p : reference->points)
    {
        std::vector<int> indices;
        std::vector<float> squaredDistances;
 
        pcl::PointXYZL q;
        q.x = p.x;
        q.y = p.y;
        q.z = p.z;
 
        if (octree.nearestKSearch(q, 1, indices, squaredDistances) > 0 && squaredDistances[0] < 50)
        {
            if (labelMap.find(p.label) == labelMap.end())
            {
                labelMap[p.label] = labeled->points[indices[0]].label;
            }
            if (labelMap[p.label] == labeled->points[indices[0]].label)
            {
                num_covered++;
            }
        }
    }
 
    return ((float)num_covered) / reference->size();
}
 
void
PrimitiveExtractionParameterTuning::generateNewParameterSet()
{
    // Randomly sample within a hyperplane around the current parameter set
    extractionPrm.minSegmentSize =
        sample(bestExtractionPrm.minSegmentSize, bounds_segmentation_minSegmentSize);
    extractionPrm.maxSegmentSize =
        sample(bestExtractionPrm.maxSegmentSize, bounds_segmentation_maxSegmentSize);
    extractionPrm.euclideanClusteringTolerance =
        sample(bestExtractionPrm.euclideanClusteringTolerance,
               bounds_segmentation_euclideanClusteringTolerance);
    extractionPrm.outlierThreshold =
        sample(bestExtractionPrm.outlierThreshold, bounds_segmentation_outlierThreshold);
    extractionPrm.planeMaxIterations =
        sample(bestExtractionPrm.planeMaxIterations, bounds_segmentation_planeMaxIterations);
    extractionPrm.planeDistanceThreshold = sample(bestExtractionPrm.planeDistanceThreshold,
                                                  bounds_segmentation_planeDistanceThreshold);
    extractionPrm.planeNormalDistance =
        sample(bestExtractionPrm.planeNormalDistance, bounds_segmentation_planeNormalDistance);
    extractionPrm.cylinderMaxIterations =
        sample(bestExtractionPrm.cylinderMaxIterations, bounds_segmentation_cylinderMaxIterations);
    extractionPrm.cylinderDistanceThreshold = sample(bestExtractionPrm.cylinderDistanceThreshold,
                                                     bounds_segmentation_cylinderDistanceThreshold);
    extractionPrm.cylinderRadiusLimit =
        sample(bestExtractionPrm.cylinderRadiusLimit, bounds_segmentation_cylinderRadiusLimit);
    extractionPrm.sphereMaxIterations =
        sample(bestExtractionPrm.sphereMaxIterations, bounds_segmentation_sphereMaxIterations);
    extractionPrm.sphereDistanceThreshold = sample(bestExtractionPrm.sphereDistanceThreshold,
                                                   bounds_segmentation_sphereDistanceThreshold);
    extractionPrm.sphereNormalDistance =
        sample(bestExtractionPrm.sphereNormalDistance, bounds_segmentation_sphereNormalDistance);
    extractionPrm.circularDistanceThreshold = sample(bestExtractionPrm.circularDistanceThreshold,
                                                     bounds_segmentation_circularDistanceThreshold);
 
    segmentationPrm.minSegmentSize =
        sample(bestSegmentationPrm.minSegmentSize, bounds_primitives_minSegmentSize);
    segmentationPrm.voxelResolution =
        sample(bestSegmentationPrm.voxelResolution, bounds_primitives_voxelResolution);
    segmentationPrm.seedResolution =
        sample(bestSegmentationPrm.seedResolution, bounds_primitives_seedResolution);
    segmentationPrm.colorImportance =
        sample(bestSegmentationPrm.colorImportance, bounds_primitives_colorImportance);
    segmentationPrm.spatialImportance =
        sample(bestSegmentationPrm.spatialImportance, bounds_primitives_spatialImportance);
    segmentationPrm.normalImportance =
        sample(bestSegmentationPrm.normalImportance, bounds_primitives_normalImportance);
    segmentationPrm.concavityThreshold =
        sample(bestSegmentationPrm.concavityThreshold, bounds_primitives_concavityThreshold);
    segmentationPrm.smoothnessThreshold =
        sample(bestSegmentationPrm.smoothnessThreshold, bounds_primitives_smoothnessThreshold);
 
    std::stringstream s;
    s << "Randomly determined parameter set: {" << std::endl
      << "    extractionPrm.minSegmentSize: " << extractionPrm.minSegmentSize << "," << std::endl
      << "    extractionPrm.maxSegmentSize: " << extractionPrm.maxSegmentSize << "," << std::endl
      << "    extractionPrm.euclideanClusteringTolerance: "
      << extractionPrm.euclideanClusteringTolerance << "," << std::endl
      << "    extractionPrm.outlierThreshold: " << extractionPrm.outlierThreshold << ","
      << std::endl
      << "    extractionPrm.planeMaxIterations: " << extractionPrm.planeMaxIterations << ","
      << std::endl
      << "    extractionPrm.planeDistanceThreshold: " << extractionPrm.planeDistanceThreshold << ","
      << std::endl
      << "    extractionPrm.planeNormalDistance: " << extractionPrm.planeNormalDistance << ","
      << std::endl
      << "    extractionPrm.cylinderMaxIterations: " << extractionPrm.cylinderMaxIterations << ","
      << std::endl
      << "    extractionPrm.cylinderDistanceThreshold: " << extractionPrm.cylinderDistanceThreshold
      << "," << std::endl
      << "    extractionPrm.cylinderRadiusLimit: " << extractionPrm.cylinderRadiusLimit << ","
      << std::endl
      << "    extractionPrm.sphereMaxIterations: " << extractionPrm.sphereMaxIterations << ","
      << std::endl
      << "    extractionPrm.sphereDistanceThreshold: " << extractionPrm.sphereDistanceThreshold
      << "," << std::endl
      << "    extractionPrm.sphereNormalDistance: " << extractionPrm.sphereNormalDistance << ","
      << std::endl
      << "    extractionPrm.circularDistanceThreshold: " << extractionPrm.circularDistanceThreshold
      << "," << std::endl
      << "    segmentationPrm.minSegmentSize: " << segmentationPrm.minSegmentSize << ","
      << std::endl
      << "    segmentationPrm.voxelResolution: " << segmentationPrm.voxelResolution << ","
      << std::endl
      << "    segmentationPrm.seedResolution: " << segmentationPrm.seedResolution << ","
      << std::endl
      << "    segmentationPrm.colorImportance: " << segmentationPrm.colorImportance << ","
      << std::endl
      << "    segmentationPrm.spatialImportance: " << segmentationPrm.spatialImportance << ","
      << std::endl
      << "    segmentationPrm.normalImportance: " << segmentationPrm.normalImportance << ","
      << std::endl
      << "    segmentationPrm.concavityThreshold: " << segmentationPrm.concavityThreshold << ","
      << std::endl
      << "    segmentationPrm.smoothnessThreshold: " << segmentationPrm.smoothnessThreshold << ","
      << std::endl
      << "}";
    ARMARX_INFO << s.str();
}
 
float
PrimitiveExtractionParameterTuning::sample(float current, const Eigen::Vector2f& bounds)
{
    float radius_percent = 0.05;
 
    if (iteration % 5 == 0)
    {
        radius_percent = 0.3;
    }
 
    // Random value in [-1, 1]
    float r = ((float)(rand() % 2000 - 1000)) / 1000.0f;
 
    // Scale to admitted radius
    current += r * (bounds(1) - bounds(0)) * radius_percent;
 
    // Crop to boundaries
    current = std::max(current, bounds(0));
    current = std::min(current, bounds(1));
 
    return current;
}
 
void
PrimitiveExtractionParameterTuning::exportBestSetup(
    const std::string& directory,
    const pcl::PointCloud<pcl::PointXYZL>::Ptr& pointCloud,
    const visionx::LccpParameters& lccp_prm,
    const visionx::PrimitiveExtractorParameters& pe_prm)
{
    std::stringstream path_pcd;
    path_pcd << directory << "/point_cloud_" << iteration << ".pcd";
 
    std::stringstream path_prm;
    path_prm << directory << "/parameters_" << iteration << ".cfg";
 
    ARMARX_INFO << "Exporting labeled point cloud to: " << path_pcd.str();
    pcl::io::savePCDFile<pcl::PointXYZL>(path_pcd.str(), *pointCloud);
 
    ARMARX_INFO << "Exporting parameters to: " << path_prm.str();
    std::ofstream f;
    f.open(path_prm.str());
 
    f << "Randomly determined parameter set: {" << std::endl
      << "    extractionPrm.minSegmentSize: " << pe_prm.minSegmentSize << "," << std::endl
      << "    extractionPrm.maxSegmentSize: " << pe_prm.maxSegmentSize << "," << std::endl
      << "    extractionPrm.euclideanClusteringTolerance: " << pe_prm.euclideanClusteringTolerance
      << "," << std::endl
      << "    extractionPrm.outlierThreshold: " << pe_prm.outlierThreshold << "," << std::endl
      << "    extractionPrm.planeMaxIterations: " << pe_prm.planeMaxIterations << "," << std::endl
      << "    extractionPrm.planeDistanceThreshold: " << pe_prm.planeDistanceThreshold << ","
      << std::endl
      << "    extractionPrm.planeNormalDistance: " << pe_prm.planeNormalDistance << "," << std::endl
      << "    extractionPrm.cylinderMaxIterations: " << pe_prm.cylinderMaxIterations << ","
      << std::endl
      << "    extractionPrm.cylinderDistanceThreshold: " << pe_prm.cylinderDistanceThreshold << ","
      << std::endl
      << "    extractionPrm.cylinderRadiusLimit: " << pe_prm.cylinderRadiusLimit << "," << std::endl
      << "    extractionPrm.sphereMaxIterations: " << pe_prm.sphereMaxIterations << "," << std::endl
      << "    extractionPrm.sphereDistanceThreshold: " << pe_prm.sphereDistanceThreshold << ","
      << std::endl
      << "    extractionPrm.sphereNormalDistance: " << pe_prm.sphereNormalDistance << ","
      << std::endl
      << "    extractionPrm.circularDistanceThreshold: " << pe_prm.circularDistanceThreshold << ","
      << std::endl
      << "    segmentationPrm.minSegmentSize: " << lccp_prm.minSegmentSize << "," << std::endl
      << "    segmentationPrm.voxelResolution: " << lccp_prm.voxelResolution << "," << std::endl
      << "    segmentationPrm.seedResolution: " << lccp_prm.seedResolution << "," << std::endl
      << "    segmentationPrm.colorImportance: " << lccp_prm.colorImportance << "," << std::endl
      << "    segmentationPrm.spatialImportance: " << lccp_prm.spatialImportance << "," << std::endl
      << "    segmentationPrm.normalImportance: " << lccp_prm.normalImportance << "," << std::endl
      << "    segmentationPrm.concavityThreshold: " << lccp_prm.concavityThreshold << ","
      << std::endl
      << "    segmentationPrm.smoothnessThreshold: " << lccp_prm.smoothnessThreshold << ","
      << std::endl
      << "}" << std::endl
      << std::endl
      << "fitness: " << bestFitness << std::endl;
 
    f.close();
}
 
std::string
PrimitiveExtractionParameterTuning::GetDefaultName()
{
    return "PrimitiveExtractionParameterTuning";
}
 
std::string
PrimitiveExtractionParameterTuning::getDefaultName() const
{
    return GetDefaultName();
}
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::PrimitiveExtractionParameterTuning, ::armarx::PrimitiveExtractionParameterTuning::GetDefaultName());