Pcl.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms of the
 * BSD-3-Clause license. See the accompanying LICENSE file for details.
 */
 
#ifndef YARP_PCL_PCL_H
#define YARP_PCL_PCL_H
 
#include <pcl/io/pcd_io.h>
#include <yarp/sig/PointCloud.h>
 
namespace yarp
{
namespace pcl
{
 
template< class T1, class T2 >
inline bool toPCL(const yarp::sig::PointCloud< T1 > &yarpCloud, ::pcl::PointCloud< T2 > &pclCloud)
{
    static_assert(sizeof(T1) == sizeof(T2), "yarp::pcl::toPcl: T1 and T2 are incompatible");
    pclCloud.points.resize(yarpCloud.size());
    pclCloud.width    = yarpCloud.width();
    pclCloud.height   = yarpCloud.height();
    pclCloud.is_dense = yarpCloud.isDense();
    memcpy((char*)& pclCloud.points.at(0), yarpCloud.getRawData(), yarpCloud.dataSizeBytes());
    return true;
}
 
template< class T1, class T2 >
inline bool fromPCL(const ::pcl::PointCloud< T1 > &pclCloud, yarp::sig::PointCloud< T2 > &yarpCloud)
{
    static_assert(sizeof(T1) == sizeof(T2), "yarp::pcl::fromPCL: T1 and T2 are incompatible");
    yarpCloud.fromExternalPC((char*) &pclCloud(0,0), yarpCloud.getPointType(), pclCloud.width, pclCloud.height, pclCloud.is_dense);
    return true;
}
 
template< class T1, class T2 >
inline int savePCD(const std::string &file_name, const yarp::sig::PointCloud< T1 > &yarpCloud)
{
    static_assert(sizeof(T1) == sizeof(T2), "yarp::pcl::savePCD: T1 and T2 are incompatible");
    ::pcl::PointCloud<T2> pclCloud(yarpCloud.width(), yarpCloud.height());
    yarp::pcl::toPCL< T1, T2 >(yarpCloud, pclCloud);
    return ::pcl::io::savePCDFile(file_name, pclCloud);
}
 
template< class T1, class T2 >
inline int loadPCD(const std::string &file_name, yarp::sig::PointCloud<T2> &yarpCloud)
{
    static_assert(sizeof(T1) == sizeof(T2), "yarp::pcl::loadPCD: T1 and T2 are incompatible");
    ::pcl::PointCloud<T1> pclCloud;
    int ret = ::pcl::io::loadPCDFile(file_name, pclCloud);
    yarp::pcl::fromPCL< T1, T2 >(pclCloud, yarpCloud);
    return ret;
}
 
} // namespace pcl
} // namespace yarp
 
 
#endif