PCL PointCloud to Image class

CMakeLists.txt

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)

project(keypoints)

set(CMAKE_CXX_STANDARD 11)

find_package(PCL 1.3 REQUIRED)
find_package(OpenCV REQUIRED)

include_directories(
  include
  ${PCL_INCLUDE_DIRS}
)

link_directories(${PCL_LIBRARY_DIRS})

add_definitions(${PCL_DEFINITIONS})

add_executable(pc2img src/pc2img_example.cpp src/pc2img.cpp)
target_link_libraries (pc2img ${PCL_LIBRARIES} ${OpenCV_LIBS})

pc2img.cpp

/**
*   Author: Lucas Amparo Barbosa
*   Date:   2019-11-13
*/
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>

#include "pc2img.h"

PointCloud2Image::PointCloud2Image(PointCloudT::Ptr cloud) {
  this->cloud_ = cloud;
}

PointCloud2Image::~PointCloud2Image() {}

PointCloudT::Ptr PointCloud2Image::projectToPlane(PointCloudT::Ptr cloud, Vec3f origin, Vec3f normal) {

  PointCloudT::Ptr aux_cloud(new PointCloudT()), output(new PointCloudT());
  pcl::copyPointCloud(*cloud, *aux_cloud);

  Eigen::Hyperplane<float, 3> plane(normal, origin);

  for (auto itPoint = aux_cloud->begin(); itPoint != aux_cloud->end(); itPoint++) {
    // project point to plane
    auto proj = plane.projection(itPoint->getVector3fMap());
    itPoint->getVector3fMap() = proj;
    // optional: save the reconstruction information as normals in the projected cloud
    itPoint->getNormalVector3fMap() = itPoint->getVector3fMap() - proj;
  }

  // Always return the plane aligned with XY plane
  Vec3f unitz = Eigen::Vector3f::UnitZ();

  Vec3f rotation_vector = normal.cross(unitz);

  Vec3f unit_rot_vector = rotation_vector / rotation_vector.norm();

  Eigen::Affine3f tf = Eigen::Affine3f::Identity();
  float theta = -std::acos(normal.dot(unitz));
  if (theta != 0)
    tf.rotate(Eigen::AngleAxisf(theta, unit_rot_vector));

  pcl::transformPointCloud(*aux_cloud, *output, tf);
  
  return output;
}

cv::Mat PointCloud2Image::planeToImage(PointCloudT::Ptr cloud, int image_size) {
  PointT min, max;
  pcl::getMinMax3D(*cloud, min, max);

  Vec3f major = max.getVector3fMap() - min.getVector3fMap();

  int width = 0, height = 0;
  double ratio = 0;
  if(major[0] > major[1]) {
    ratio = major[1]/major[0];
    width = image_size;
    height = round(ratio * image_size);
  } else {
    ratio = major[0]/major[1];
    width = round(ratio * image_size);
    height = image_size;
  }

  cv::Mat image(width, height, CV_8U);
  image.setTo(0);
  for(PointT p : cloud->points) {
    int x = ((p.x - min.x) / (max.x - min.x)) * width;
    int y = ((p.y - min.y) / (max.y - min.y)) * height;

    image.at<uchar>(x, y) = (uchar) p.r;
  } 

  return image;
}

void PointCloud2Image::convert(Vec3f origin, Vec3f normal, float image_size) {
  this->plane_ = this->projectToPlane(this->cloud_, origin, normal);

  this->image_ = this->planeToImage(this->plane_, image_size);
}

cv::Mat PointCloud2Image::retrieveImage() {
  return this->image_;
}

PointCloudT::Ptr PointCloud2Image::retrievePlaneCloud() {
  return this->plane_;
}

pc2img.h

/**
*   Author: Lucas Amparo Barbosa
*   Date:   2019-11-13
*/

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <opencv2/opencv.hpp>

typedef pcl::PointXYZRGBNormal          PointT;
typedef Eigen::Vector3f                 Vec3f;

typedef pcl::PointCloud<PointT>         PointCloudT;

class PointCloud2Image {
 public:
  PointCloud2Image(PointCloudT::Ptr cloud);
  ~PointCloud2Image();

  void convert(Vec3f origin, Vec3f normal, float image_size);
  cv::Mat retrieveImage();
  PointCloudT::Ptr retrievePlaneCloud();

 private:
  PointCloudT::Ptr projectToPlane(PointCloudT::Ptr cloud, Vec3f origin, Vec3f normal);
  cv::Mat planeToImage(PointCloudT::Ptr cloud, int image_size);

  PointCloudT::Ptr cloud_, plane_;
  cv::Mat image_;
};

pc2img_example.cpp

#include "pc2img.h"

#include <pcl/features/normal_3d_omp.h>

PointCloudT::Ptr computeNormals(PointCloudT::Ptr cloud_, double radius = 0.05) {
  pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;
  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>());

  ne.setSearchMethod(tree);
  ne.setInputCloud(cloud_);
  ne.setRadiusSearch(radius);
  ne.compute(*cloud_normals);

  pcl::concatenateFields(*cloud_, *cloud_normals, *cloud_);

  return cloud_;
}

int main(int argc, char ** argv) {
  pcl::PCDWriter writer;
  pcl::PCDReader reader;

  PointCloudT::Ptr cloudA(new PointCloudT());
  reader.read(argv[1], *cloudA);
  
  PointCloud2Image converter(computeNormals(cloudA));
  converter.convert(Eigen::Vector3f(0, 0, 0), Eigen::Vector3f::UnitZ(), 500);

  cv::Mat image = converter.retrieveImage();

  // writer.write("plane.pcd", *(converter.retrievePlaneCloud()));

  cv::imshow("image", image);
  cv::imwrite("image.png", image);
  cv::waitKey(0);

  return 0;
}