Skip to content

Instantly share code, notes, and snippets.

@lucasamparo
Last active November 14, 2019 14:02
Show Gist options
  • Save lucasamparo/9f6d13d6a8824b698e9a88b25fe7fdb4 to your computer and use it in GitHub Desktop.
Save lucasamparo/9f6d13d6a8824b698e9a88b25fe7fdb4 to your computer and use it in GitHub Desktop.
PCL PointCloud to Image class
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})
/**
* 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_;
}
/**
* 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_;
};
#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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment