rock_to_pcl.cpp

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <base/samples/Pointcloud.hpp>

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

void pointcloud_rock_to_pcl(const base::samples::Pointcloud &cloud_in, PointCloudT::Ptr cloud_out)
{
    // copy meta data
    // cloud_out->header.frame_id = ?;
    cloud_out->header.stamp = cloud_in.time.toMicroseconds();
    if (cloud_in.points.size() == 640 * 480)    // Kinect/Xtion hack
    {
        cloud_out->height = 480;
        cloud_out->width = 640;
    } else if (cloud_in.points.size() == 320 * 240) {
        cloud_out->height = 240;
        cloud_out->width = 320;
    } else {
        cloud_out->height = 1;
        cloud_out->width = cloud_in.points.size();
    }
    cloud_out->is_dense = true;   // will be updated later
    //cloud_out->sensor_orientation_ = ?;
    //cloud_out->sensor_origin_ = ?;

    // copy points
    cloud_out->points.resize(cloud_in.points.size());

    for (size_t i = 0; i < cloud_in.points.size(); ++i)
    {
        const base::Point &p_in = cloud_in.points[i];
        const base::Vector4d &c_in = cloud_in.colors[i];
        PointT &p_out = cloud_out->points[i];

        if (!std::isfinite(p_in.x()) ||
            !std::isfinite(p_in.y()) ||
            !std::isfinite(p_in.z()))
        {
            // point is invalid
            static float nan = std::numeric_limits<float>::quiet_NaN();
            p_out.x = nan;
            p_out.y = nan;
            p_out.z = nan;
            cloud_out->is_dense = false;
        } else
        {
            // point is valid
            p_out.x = p_in.x();
            p_out.y = p_in.y();
            p_out.z = p_in.z();
        }

        p_out.r = c_in.x() * 255.0;
        p_out.g = c_in.y() * 255.0;
        p_out.b = c_in.z() * 255.0;
    }
}