lucasamparo · November 14, 2019 14:02 · Nov 14, 2019 · Nov 14, 2019
diff --git a/pc2img.cpp b/pc2img.cpp
@@ -41,8 +41,6 @@ PointCloudT::Ptr PointCloud2Image::projectToPlane(PointCloudT::Ptr cloud, Vec3f
   if (theta != 0)
     tf.rotate(Eigen::AngleAxisf(theta, unit_rot_vector));
 
-  std::cout << tf.matrix() << std::endl;
-
   pcl::transformPointCloud(*aux_cloud, *output, tf);
 
   return output;

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -0,0 +1,20 @@
+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})
diff --git a/pc2img.cpp b/pc2img.cpp
@@ -0,0 +1,93 @@
+/**
+*   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));
+
+  std::cout << tf.matrix() << std::endl;
+
+  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_;
+}
diff --git a/pc2img.h b/pc2img.h
@@ -0,0 +1,31 @@
+/**
+*   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_;
+};
diff --git a/pc2img_example.cpp b/pc2img_example.cpp
@@ -0,0 +1,39 @@
+#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;
+}