pcl_conversions.cpp

Go to the documentation of this file.

#include "pcl_conversions.h"
 
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include <cartographer/sensor/rangefinder_point.h>
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
    template <typename T>
    ::pcl::PointCloud<::pcl::PointXYZ>
    toPCLImpl(const std::vector<T>& points)
    {
        ::pcl::PointCloud<::pcl::PointXYZ> cloud;
        cloud.reserve(points.size());
 
        std::transform(
            points.begin(),
            points.end(),
            std::back_inserter(cloud.points),
            [](const auto& point) {
                return pcl::PointXYZ{point.position.x(), point.position.y(), point.position.z()};
            });
 
        // @see pcl::PointCloud::push_back
        cloud.height = 1;
        cloud.width = cloud.size();
 
        return cloud;
    }
 
    ::pcl::PointCloud<::pcl::PointXYZ>
    toPCL(const std::vector<::cartographer::sensor::TimedRangefinderPoint>& points)
    {
        return toPCLImpl(points);
    }
 
    ::pcl::PointCloud<::pcl::PointXYZ>
    toPCL(const std::vector<::cartographer::sensor::RangefinderPoint>& points)
    {
        return toPCLImpl(points);
    }
 
    ::pcl::PointCloud<::pcl::PointXYZ>
    toPCL(const auto& pointCloud, const ::cartographer::transform::Rigid3d& pose)
    {
        ::pcl::PointCloud<pcl::PointXYZ> cloudOut;
        cloudOut.reserve(pointCloud.points().size());
 
        auto cloudTmp = toPCL(pointCloud);
 
        const Eigen::Quaternionf q = pose.rotation().cast<float>();
        const Eigen::Vector3f t = pose.translation().cast<float>();
 
        pcl::transformPointCloud(cloudTmp, cloudOut, t, q);
        return cloudOut;
    }
 
    ::pcl::PointCloud<::pcl::PointXYZ>
    toPCL(const std::vector<::cartographer::sensor::TimedRangefinderPoint>& pointCloud,
          const Eigen::Isometry3f& pose)
    {
        ::pcl::PointCloud<pcl::PointXYZ> cloudOut;
        cloudOut.reserve(pointCloud.size());
 
        auto cloudTmp = toPCL(pointCloud);
 
        ::pcl::transformPointCloud(cloudTmp, cloudOut, pose);
        return cloudOut;
    }
 
} // namespace armarx::localization_and_mapping::cartographer_adapter