PointCloudConversions.h

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::Tools
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <cassert>
#include <iostream>
 
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
 
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
#include <VisionX/interface/core/DataTypes.h>
 
 
namespace visionx::tools
{
 
    template <typename PointT>
    visionx::PointContentType getPointContentType()
    {
        return visionx::ePoints;
    }
 
    template <> visionx::PointContentType getPointContentType<pcl::PointXYZ>();
    template <> visionx::PointContentType getPointContentType<pcl::PointXYZI>();
    template <> visionx::PointContentType getPointContentType<pcl::PointXYZL>();
    template <> visionx::PointContentType getPointContentType<pcl::PointNormal>();
    template <> visionx::PointContentType getPointContentType<pcl::PointXYZRGBA>();
    template <> visionx::PointContentType getPointContentType<pcl::PointXYZRGBL>();
    template <> visionx::PointContentType getPointContentType<pcl::PointXYZRGBNormal>();
 
 
    size_t getBytesPerPoint(visionx::PointContentType pointContent);
    std::string toString(visionx::PointContentType pointContent);
 
 
    /// Indicate whether this point type contains colors.
    bool isColored(visionx::PointContentType type);
    /// Indicate whether this point type contains labels.
    bool isLabeled(visionx::PointContentType type);
 
 
    /**
     * @param sourcePtr point to the source pcl point cloud
     */
    //template<typename PointT>
    // void convertFromPCL(typename pcl::PointCloud<PointT>::Ptr sourcePtr, void **target)
    void convertFromPCL(pcl::PointCloud<pcl::PointXYZ>::Ptr sourcePtr, void** target);
 
 
    /**
     * @param targetPtr point to the pcl point cloud
     */
    //template<typename PointT>
    //void convertToPCL(void **source, visionx::MetaPointCloudFormatPtr pointCloudFormat, typename pcl::PointCloud<PointT>::Ptr targetPtr)
    void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZ>::Ptr targetPtr);
 
    void convertToPCL(const visionx::ColoredPointCloud& source, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr targetPtr);
    void convertFromPCL(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr sourcePtr, visionx::ColoredPointCloud& target);
 
    void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr sourcePtr, void** target);
    void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr targetPtr);
 
    void convertFromPCL(pcl::PointCloud<pcl::PointXYZL>::Ptr sourcePtr, void** target);
    void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZL>::Ptr targetPtr);
 
    void convertFromPCL(pcl::PointCloud<pcl::PointXYZI>::Ptr sourcePtr, void** target);
    void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZI>::Ptr targetPtr);
 
 
    void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBL>::Ptr sourcePtr, void** target);
    void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZRGBL>::Ptr targetPtr);
 
    void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& sourcePtr, void** target);
    void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr targetPtr);
 
    void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGB>::Ptr sourcePtr, void** target);
    void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetPtr);
 
    enum PCLToDebugDrawerConversionMode
    {
        eConvertAsPoints,
        eConvertAsColors,
        eConvertAsLabels,
    };
 
    void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& sourcePtr, armarx::DebugDrawer24BitColoredPointCloud& target, PCLToDebugDrawerConversionMode mode = eConvertAsPoints, float pointSize = 3);
    void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZL>::Ptr& sourcePtr, armarx::DebugDrawer24BitColoredPointCloud& target, PCLToDebugDrawerConversionMode mode = eConvertAsPoints, float pointSize = 3);
    void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZ>::Ptr& sourcePtr, armarx::DebugDrawer24BitColoredPointCloud& target, PCLToDebugDrawerConversionMode mode = eConvertAsPoints, float pointSize = 3);
    void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZRGBL>::Ptr& sourcePtr, armarx::DebugDrawer24BitColoredPointCloud& target, PCLToDebugDrawerConversionMode mode = eConvertAsPoints, float pointSize = 3);
 
 
 
    // TEMPLATE BASED POINT CLOUD CONVERSION between PCL and VisionX (Ice) types.
    // (without copying code over and over)
 
    /// Convert a PCL point cloud to a VisionX point cloud.
    template <typename VisionXPointT, typename PclPointT>
    void convertFromPCL(const pcl::PointCloud<PclPointT>& source,
                        std::vector<VisionXPointT>& target);
 
    /**
     * @brief Convert a PCL point cloud to a VisionX point cloud (as return value).
     *
     * Pass the desired point type as first template argument, for example:
     * @code
     *   pcl::PointCloud<pcl::PointXYZRGBA> source = ...;
     *   ColoredPointCloud target = visionx::tools::convertFromPCL<ColoredPoint3D>(source);
     *   ColoredPointCloud target = visionx::tools::convertFromPCL<ColoredPointCloud::value_type>(source);
     * @endcode
     */
    template <typename VisionXPointT, typename PclPointT>
    std::vector<VisionXPointT> convertFromPCL(const pcl::PointCloud<PclPointT>& source);
 
 
    /// Convert a VisionX point cloud to a PCL point cloud.
    template <typename PclPointT, typename VisionXPointT>
    void convertToPCL(const std::vector<VisionXPointT>& source,
                      pcl::PointCloud<PclPointT>& target);
 
    /**
     * @brief Convert a VisionX point cloud to a PCL point cloud (as return value).
     *
     * Pass the desired point type as first template argument, for example:
     * @code
     *   using PointCloudT = pcl::PointCloud<pcl::PointXYZRGBA>;
     *   ColoredPointCloud source = ...;
     *   pcl::PointCloud<pcl::PointXYZRGBA>::Ptr target = visionx::tools::convertToPCL<pcl::PointXYZRGBA>(source);
     *   pcl::PointCloud<pcl::PointXYZRGBA> target = *visionx::tools::convertToPCL<PointCloudT::PointType>(source);
     * @endcode
     */
    template <typename PclPointT, typename VisionXPointT>
    typename pcl::PointCloud<PclPointT>::Ptr convertToPCL(const std::vector<VisionXPointT>& source);
 
 
    // POINT CONVERSION
 
    /// Convert a PCL point type to a VisionX point type.
    template <typename PclPointT, typename VisionXPointT>
    void convertPointFromPCL(const PclPointT& p, VisionXPointT& v)
    {
        p.CONVERSION_BETWEEN_THESE_POINT_TYPES_IS_NOT_IMPLEMENTED(v);
    }
 
    /// Convert a VisionX point type to a PCL point type.
    template <typename PclPointT, typename VisionXPointT>
    void convertPointToPCL(const VisionXPointT& v, PclPointT& p)
    {
        p.CONVERSION_BETWEEN_THESE_POINT_TYPES_IS_NOT_IMPLEMENTED(v);
    }
 
    // POINT CONVERSION HELPERS
}
 
namespace visionx::tools::detail
{
    /// Convert x, y, z attributes. Use with visionx::Point3D and PCL type with XYZ.
    template <typename SourceT, typename TargetT>
    void convertXYZ(const SourceT& source, TargetT& target)
    {
        target.x = source.x;
        target.y = source.y;
        target.z = source.z;
    }
 
    /// Convert r, g, b attributes. Use with visionx::RGBA and PCL type with RGBA.
    template <typename SourceT, typename TargetT>
    void convertRGBA(const SourceT& source, TargetT& target)
    {
        target.r = source.r;
        target.g = source.g;
        target.b = source.b;
        target.a = source.a;
    }
 
    /// Convert label attribute. Use with visionx point type and PCL type with labels.
    template <typename SourceT, typename TargetT>
    void convertLabel(const SourceT& source, TargetT& target)
    {
        target.label = source.label;
    }
}
 
namespace visionx::tools
{
 
    // POINT CONVERSION SPECIALIZATIONS
 
    // pcl::PointXYZ <-> visionx::Point3D
    template <> void convertPointFromPCL(const pcl::PointXYZ& source, visionx::Point3D& target);
    template <> void convertPointToPCL(const visionx::Point3D& source, pcl::PointXYZ& target);
 
    // pcl::PointXYZRGBA <-> visionx::ColoredPoint3D
    template <> void convertPointFromPCL(const pcl::PointXYZRGBA& source, visionx::ColoredPoint3D& target);
    template <> void convertPointToPCL(const visionx::ColoredPoint3D& source, pcl::PointXYZRGBA& target);
 
    // pcl::PointXYZL <-> visionx::LabeledPoint3D
    template <> void convertPointFromPCL(const pcl::PointXYZL& source, visionx::LabeledPoint3D& target);
    template <> void convertPointToPCL(const visionx::LabeledPoint3D& source, pcl::PointXYZL& target);
 
    // pcl::PointXYZRGBL <-> visionx::ColoredLabeledPoint3D
    template <> void convertPointFromPCL(const pcl::PointXYZRGBL& source, visionx::ColoredLabeledPoint3D& target);
    template <> void convertPointToPCL(const visionx::ColoredLabeledPoint3D& source, pcl::PointXYZRGBL& target);
 
 
 
    // IMPLEMENTATION (depends on convertPointFromPcl)
 
    // Convert a PCL point cloud to a VisionX point cloud.
    template <typename VisionXPointT, typename PclPointT>
    void convertFromPCL(const pcl::PointCloud<PclPointT>& source,
                        std::vector<VisionXPointT>& target)
    {
        target.resize(source.size());
        for (std::size_t i = 0; i < source.size(); ++i)
        {
            convertPointFromPCL(source[i], target[i]);
        }
    }
 
    // Convert a PCL point cloud to a VisionX point cloud.
    template <typename VisionXPointT, typename PclPointT>
    std::vector<VisionXPointT> convertFromPCL(const pcl::PointCloud<PclPointT>& source)
    {
        std::vector<VisionXPointT> target;
        convertFromPCL(source, target);
        return target;
    }
 
    // Convert a VisionX point cloud to a PCL point cloud.
    template <typename PclPointT, typename VisionXPointT>
    void convertToPCL(const std::vector<VisionXPointT>& source,
                      pcl::PointCloud<PclPointT>& target)
    {
        target.resize(source.size());
        for (std::size_t i = 0; i < source.size(); ++i)
        {
            convertPointToPCL(source[i], target[i]);
        }
    }
 
    /// Convert a VisionX point cloud to a PCL point cloud.
    template <typename PclPointT, typename VisionXPointT>
    typename pcl::PointCloud<PclPointT>::Ptr convertToPCL(const std::vector<VisionXPointT>& source)
    {
        typename pcl::PointCloud<PclPointT>::Ptr target(new pcl::PointCloud<PclPointT>);
        convertToPCL(source, *target);
        return target;
    }
 
}