point_iterator_safe.cpp

Go to the documentation of this file.

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointField.h>
#include <pcl/common/io.h>

#include "point_cloud_color/point_iterator.h"

namespace point_cloud_color {

template <bool SAFE> PointCloud2Iterator<SAFE>::PointCloud2Iterator(sensor_msgs::PointCloud2::Ptr pcl, std::vector<std::string> fieldNames):
                    pcl(pcl), fieldNames(fieldNames) {
    fieldOffsets.resize(fieldNames.size(), 0);
    fieldDataTypes.resize(fieldNames.size(), 0);
    for (size_t i = 0; i < fieldNames.size(); i++) {
        int fieldIndex = pcl::getFieldIndex(*pcl, fieldNames[i]);
        if (fieldIndex < 0) {
            fieldsAvailable[i] = false;
            fieldOffsets[i] = 0;
            fieldDataTypes[i] = 0;
            fieldDataSizes[i] = 0;
        } else {
            fieldsAvailable[i] = true;
            fieldOffsets[i] = pcl->fields[fieldIndex].offset;
            fieldDataTypes[i] = pcl->fields[fieldIndex].datatype;
            switch (pcl->fields[fieldIndex].datatype) {
            case sensor_msgs::PointField::INT8:
            case sensor_msgs::PointField::UINT8:
                fieldDataSizes[i] = 1;
                break;
            case sensor_msgs::PointField::INT16:
            case sensor_msgs::PointField::UINT16:
                fieldDataSizes[i] = 2;
                break;
            case sensor_msgs::PointField::FLOAT32:
            case sensor_msgs::PointField::INT32:
            case sensor_msgs::PointField::UINT32:
                fieldDataSizes[i] = 4;
                break;
            case sensor_msgs::PointField::FLOAT64:
                fieldDataSizes[i] = 8;
                break;
            }
        }
    }
}

//bool PointCloud2Iterator::isFieldAvailable(uint32_t fieldIndex) {
//    return fieldsAvailable[fieldIndex];
//}
//void PointCloud2Iterator::checkFieldAvailable(uint32_t fieldIndex) {
//    if (!isFieldAvailable(fieldIndex)) {
//        throw std::runtime_error("Field not available.");
//    }
//}
template <bool SAFE, class T> T PointCloud2Iterator<SAFE>::getField<T>(uint32_t pointIndex, uint32_t fieldIndex) {
    checkFieldAvailable(fieldIndex);
    return *reinterpret_cast<T*>(pcl->data.data() + pointIndex * pcl->point_step + fieldOffsets[fieldIndex]);
}
template <bool SAFE, class T> float PointCloud2Iterator<SAFE>::getField<T>(uint32_t pointIndex, uint32_t fieldIndex) {
//    checkFieldAvailable(fieldIndex);
    return *reinterpret_cast<float*>(pcl->data.data() + pointIndex * pcl->point_step + fieldOffsets[fieldIndex]);
}
template <class T> void PointCloud2Iterator<SAFE>::setField<T>(uint32_t pointIndex, uint32_t fieldIndex, T value) {
//    checkFieldAvailable(fieldIndex);
    *reinterpret_cast<T*>(pcl->data.data() + pointIndex * pcl->point_step + fieldOffsets[fieldIndex]) = value;
}
template <> void PointCloud2Iterator<SAFE>::setField<T>(uint32_t pointIndex, uint32_t fieldIndex, float value) {
//    checkFieldAvailable(fieldIndex);
    *reinterpret_cast<float*>(pcl->data.data() + pointIndex * pcl->point_step + fieldOffsets[fieldIndex]) = value;
}

} // namespace point_cloud_color

/*
void PclColor::pointCloud2Callback(const PointCloud2::ConstPtr &pclPtr) {
  size_t numPoints = pclPtr->width * pclPtr->height;
  NODELET_INFO("PclColor::pointCloud2Callback: Point cloud received (%lu points).", numPoints);

  // Transform the points to a fixed frame.
  PointCloud2::Ptr pclFixed(new PointCloud2);
  if (!transformListener.waitForTransform(fixedFrame, pclPtr->header.frame_id, pclPtr->header.stamp, ros::Duration(3.0))
      || !pcl_ros::transformPointCloud(fixedFrame, *pclPtr, *pclFixed, transformListener)) {
    NODELET_WARN("Cannot transform point cloud to the fixed frame %s.", fixedFrame.data());
    return;
  }

  // Prepare color point cloud.
  PointCloud2::Ptr pclColor(new PointCloud2);
  pclColor->header = pclPtr->header;
  pclColor->width = pclPtr->width;
  pclColor->height = pclPtr->height;
  pclColor->is_bigendian = pclPtr->is_bigendian;
  pclColor->is_dense = pclPtr->is_dense;
  PointField rgbaField;
  rgbaField.count = 1;
  rgbaField.datatype = PointField::UINT32;
  rgbaField.name = "rgba";
  rgbaField.offset = 0;
  pclColor->fields.push_back(rgbaField);
  pclColor->point_step = 4;
  pclColor->row_step = pclColor->point_step * pclColor->width;
  pclColor->data.resize(pclColor->row_step * pclColor->height);
  vector<string> pclColorFieldNames;
  pclColorFieldNames.push_back("rgba");
  PointCloud2Iterator pclColorIter(pclColor, pclColorFieldNames);

  // Initialize vector with projection distances from image center, used as a quality measure.
  vector<double> dist(numPoints, DBL_MAX);
  map<string, cv_bridge::CvImageConstPtr>::iterator itImages;
  for (itImages = images.begin(); itImages != images.end(); itImages++) {
    string cameraFrame = (*itImages).first;
    cv_bridge::CvImageConstPtr imagePtr  = images[cameraFrame];
    sensor_msgs::CameraInfoConstPtr cameraInfoPtr = cameraInfos[cameraFrame];
    NODELET_DEBUG("Map key %s, image frame %s, camera info frame %s.", cameraFrame.data(), imagePtr->header.frame_id.data(), cameraInfoPtr->header.frame_id.data());

    image_geometry::PinholeCameraModel cameraModel;
    if (!cameraModel.fromCameraInfo(cameraInfoPtr)) {
      NODELET_WARN("Camera model could not be constructed from camera info. Skipping the image...");
      continue;
    }

    // Check relative age of the point cloud and the image. Skip the image if the time span is to large.
    double imageAge = (pclPtr->header.stamp - imagePtr->header.stamp).toSec();
    if (imageAge > maxImageAge) {
      NODELET_WARN("Image %.1f s > %.1f s older than point cloud. Skipping the image...", imageAge, maxImageAge);
      continue;
    }
    NODELET_DEBUG("PclColor::pointCloud2Callback: Using camera %s, image age %.1f s.", cameraFrame.data(), imageAge);

    // Transform point cloud into camera frame, skip the image if the transform is not available.
    PointCloud2::Ptr pclCam(new PointCloud2);
    if (!transformListener.waitForTransform(cameraFrame, fixedFrame, imagePtr->header.stamp, ros::Duration(3.0))
        || !pcl_ros::transformPointCloud(cameraFrame, *pclFixed, *pclCam, transformListener)) {
      NODELET_WARN("Cannot transform point cloud to camera frame %s. Skipping the image...", cameraFrame.data());
      continue;
    }

    vector<string> pclFieldNames;
    pclFieldNames.push_back("x");
    pclFieldNames.push_back("y");
    pclFieldNames.push_back("z");
    PointCloud2Iterator pclCamIter(pclCam, pclFieldNames);
    pclCamIter.resetIterator(pclCam);
    pclColorIter.resetIterator(pclColor);

    int iPoint = 0;
    while (pclCamIter.next() && pclColorIter.next()) {
      // Skip points behind the camera.
      if (pclCamIter.getField<float>(2) < 0) {
        continue;
      }
      cv::Point3d p3 = cv::Point3d(pclCamIter.getField<float>(0), pclCamIter.getField<float>(1), pclCamIter.getField<float>(2));
      // Get rectified image coordinates.
      cv::Point2d p2 = cameraModel.project3dToPixel(p3);
      if (unrectify) {
        p2 = cameraModel.unrectifyPoint(p2);
      }
      double d = hypot(cameraInfoPtr->width / 2.0 - p2.x, cameraInfoPtr->height / 2.0 - p2.y);
      if (d >= dist[iPoint]) {
        // Keep color from the previous projection which we assume is better.
        continue;
      }
      // TODO: Linear interpolation.
      int x = round(p2.x);
      int y = round(p2.y);
      // Skip points outside the image.
      if (x < 0 || y < 0 || x >= cameraInfoPtr->width || y >= cameraInfoPtr->height) {
        continue;
      }
      dist[iPoint] = d;
      cv::Vec3b px = imagePtr->image.at<cv::Vec3b>(y, x);
      pclColorIter.setField<uint32_t>(0, uint32_t(px.val[2]) << 24 | uint32_t(px.val[1]) << 16 | uint32_t(px.val[0]) << 8 | 0xffu);
      iPoint++;
    }
  }

  // Prepare output point cloud.
  PointCloud2::Ptr pclOut(new PointCloud2);
  pcl::concatenateFields(*pclPtr, *pclColor, *pclOut);
  pointCloud2Pub.publish(pclOut);
}
 */