PointCloudUtility.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::ArmarXObjects::PointCloudUtility
 * @author     Tim Niklas Freudenstein ( uidnv at student dot kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PointCloudUtility.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
 
using namespace armarx;
 
void
PointCloudUtility::fusePointclouds(std::string file1, std::string file2, std::string out)
{
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc1 = loadPointCloud(file1);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc2 = loadPointCloud(file2);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc1Aligned =
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> filter;
 
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp1 =
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp2 =
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
 
    //    filter.setLeafSize(0.01, 0.01, 0.01);
    //    filter.setInputCloud(pc1);
    //    filter.filter(*temp1);
    //    filter.setInputCloud(pc2);
    //    filter.filter(*temp2);
 
 
    pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> aligner;
    aligner.setEuclideanFitnessEpsilon(1e-20);
    aligner.setTransformationEpsilon(1e-20);
    aligner.setMaximumIterations(10000000);
    aligner.setUseReciprocalCorrespondences(true);
    aligner.setInputSource(pc1);
    aligner.setInputTarget(pc2);
    aligner.align(*pc1Aligned);
    pcl::PointCloud<pcl::PointXYZRGBA> pcout = *pc2 + *pc1Aligned;
    ARMARX_VERBOSE << aligner.getFitnessScore();
    pcl::io::savePCDFileASCII(out, pcout);
}
 
void
PointCloudUtility::moveOrigin(std::string file, std::string out, Eigen::Vector3f translation)
{
    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
    transform.block<3, 1>(0, 3) = translation;
    this->transformPointcloud(file, out, transform);
}
 
void
PointCloudUtility::rotatePointCloud(std::string file, std::string out, Eigen::Matrix3f rotation)
{
    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
    transform.block<3, 3>(0, 0) = rotation;
    this->transformPointcloud(file, out, transform);
}
 
void
PointCloudUtility::transformPointcloud(std::string file, std::string out, Eigen::Matrix4f pose)
{
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(file);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud(
        new pcl::PointCloud<pcl::PointXYZRGBA>());
    pcl::transformPointCloud<pcl::PointXYZRGBA>(*pc, *transformed_cloud, pose);
 
    pcl::io::savePCDFileASCII(out, *transformed_cloud);
}
 
void
PointCloudUtility::showResult(std::string file)
{
    pcl::visualization::PCLVisualizer viewer;
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(file);
    viewer.addPointCloud(pc);
    viewer.addCoordinateSystem(1.0);
    viewer.spin();
    while (!viewer.wasStopped())
    {
    }
}
 
void
PointCloudUtility::process()
{
    std::string file1 = getProperty<std::string>("PointCloud1").getValue();
    std::string file2 = getProperty<std::string>("PointCloud2").getValue();
    std::string out = getProperty<std::string>("ResultFileName").getValue();
    bool merge = getProperty<bool>("Merge").getValue();
    bool transform = getProperty<bool>("Transform").getValue();
    bool view = getProperty<bool>("Show").getValue();
    std::string temp = "temp.pcd";
    ARMARX_VERBOSE << "Starting";
    if (merge)
    {
 
        fusePointclouds(file1, file2, temp);
        ARMARX_VERBOSE << "Merged";
    }
    else
    {
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(file1);
        pcl::io::savePCDFileASCII(temp, *pc);
    }
    if (transform)
    {
        Eigen::Vector3f tVector;
        tVector << getProperty<float>("X").getValue(), getProperty<float>("Y").getValue(),
            getProperty<float>("Z").getValue();
        Eigen::Vector3f rot;
        rot << getProperty<float>("Roll").getValue(), getProperty<float>("Pitch").getValue(),
            getProperty<float>("Yaw").getValue();
        Eigen::Matrix4f transpose;
        VirtualRobot::MathTools::posrpy2eigen4f(tVector, rot, transpose);
        transformPointcloud(temp, out, transpose);
        ARMARX_VERBOSE << "Transformed.";
    }
    else
    {
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc = loadPointCloud(temp);
        pcl::io::savePCDFileASCII(out, *pc);
    }
    if (view)
    {
        showResult(out);
    }
    //auto manager = getArmarXManager();
    //auto name = getName();
    //std::thread {[manager, name]{manager->removeObjectBlocking(name);}}.detach();
}
 
void
PointCloudUtility::onInitComponent()
{
    processorTask = new RunningTask<PointCloudUtility>(this, &PointCloudUtility::process);
}
 
/**
 * @brief PointCloudHandLocalizer::loadPointCloud loads a point cloud file from
 * the ArmarXDataPath. Returns nullptr if failed.
 * @param filename the name of the pointcloud file
 * @return a pointer to said pointcload
 */
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
PointCloudUtility::loadPointCloud(std::string filename)
{
 
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr model(new pcl::PointCloud<pcl::PointXYZRGBA>());
    if (!ArmarXDataPath::getAbsolutePath(filename, filename))
    {
        ARMARX_ERROR << "Could not find point cloud file in ArmarXDataPath: " << filename;
        return nullptr;
    }
    else if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(filename.c_str(), *model) == -1)
    {
        ARMARX_WARNING << "unable to load point cloud from file " << filename;
        return nullptr;
    }
    else
    {
        return model;
    }
}
 
void
PointCloudUtility::onConnectComponent()
{
}
 
void
PointCloudUtility::onDisconnectComponent()
{
}
 
void
PointCloudUtility::onExitComponent()
{
}
 
armarx::PropertyDefinitionsPtr
PointCloudUtility::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new PointCloudUtilityPropertyDefinitions(getConfigIdentifier()));
}
 
std::string
PointCloudUtility::GetDefaultName()
{
    return "PointCloudUtility";
}
 
std::string
PointCloudUtility::getDefaultName() const
{
    return GetDefaultName();
}
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::PointCloudUtility, ::armarx::PointCloudUtility::GetDefaultName());