PCLConverter.h

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/>.
*
* @author     Fabian Peller (fabian dot peller at kit dot edu)
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <numeric>
#include <string>
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
 
namespace armarx::aron::data::converter
{
    class AronPCLConverter
    {
        AronPCLConverter() = delete;
 
    public:
        template <typename T>
        static pcl::PointCloud<T>
        ConvertToPointClout(const data::NDArrayPtr& nav)
        {
            ARMARX_CHECK_NOT_NULL(nav);
 
            if (nav->getShape().size() != 3) // +1 for bytes per pixel
            {
                throw error::AronException("AronIVTConverter",
                                           "ConvertToCByteImage",
                                           "The size of an NDArray does not match.",
                                           nav->getPath());
            }
            auto dims = nav->getShape();
 
            pcl::PointCloud<T> ret(dims[0], dims[1]);
            memcpy(reinterpret_cast<unsigned char*>(ret.points.data()),
                   nav->getData(),
                   std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
            return ret;
        }
    };
 
} // namespace armarx::aron::data::converter
 
// legacy
namespace armarx::aron::converter
{
    using AronPCLConverter = ::armarx::aron::data::converter::AronPCLConverter;
}