Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Convert Utility for Point Cloud of Open3D and PCL
  • bunny.pcd
    • convert::to_pcl<PointType>( open3d_cloud ) 2020-05-27_11h46_21
    • convert::to_open3d<PointType>( pcl_cloud ) 2020-05-27_11h46_48
cmake_minimum_required( VERSION 3.6 )
# Language
enable_language( CXX )
# Compiler Settings
set( CMAKE_CXX_STANDARD 17 )
set( CMAKE_CXX_STANDARD_REQUIRED ON )
set( CMAKE_CXX_EXTENSIONS OFF )
# Project
project( convert LANGUAGES CXX )
add_executable( convert convert.h main.cpp )
# (Option) Start-Up Project for Visual Studio
set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "convert" )
# Find Package
find_package( Open3D REQUIRED )
find_package( PCL REQUIRED )
# Set Package to Project
if( Open3D_FOUND AND PCL_FOUND )
include_directories( ${Open3D_INCLUDE_DIRS} )
target_link_directories( convert PRIVATE ${Open3D_LIBRARY_DIRS} )
target_link_libraries( convert ${Open3D_LIBRARIES} )
target_link_libraries( convert ${PCL_LIBRARIES} )
endif()
find_package(OpenMP REQUIRED)
if( OpenMP_FOUND )
set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}" )
set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}" )
endif()
/*
This is utility to that provides converter to convert open3d::geometry::PointCloud and pcl::PointCloud<T>.
// Point Type (pcl::PointXYZ, pcl::PointNormal, pcl::PointXYZRGB, pcl::PointXYZRGBA, pcl::PointXYZRGBNormal)
typedef pcl::PointXYZRGB PointType;
// Convert Point Cloud To Open3D From PCL
std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud = convert::to_open3d<PointType>( pcl_cloud );
// Convert Point Cloud To PCL From Open3D
pcl::PointCloud<PointType>::Ptr pcl_cloud = convert::to_pcl<PointType>( open3d_cloud );
Copyright (c) 2020 Tsukasa Sugiura <t.sugiura0204@gmail.com>
Licensed under the MIT license.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#ifndef __CONVERT__
#define __CONVERT__
#include <vector>
#include <memory>
#define NOMINMAX
#include <Open3D/Open3D.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace convert
{
template<typename T>
std::shared_ptr<pcl::PointCloud<T>> to_pcl( const std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud )
{
pcl::PointCloud<T>::Ptr pcl_cloud = pcl::make_shared<pcl::PointCloud<T>>();
return pcl_cloud;
};
template<>
std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> to_pcl<pcl::PointXYZ>( const std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud )
{
const uint32_t size = open3d_cloud->points_.size();
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
pcl_cloud->width = size;
pcl_cloud->height = 1;
pcl_cloud->is_dense = false;
pcl_cloud->points.resize( size );
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
pcl_cloud->points[i].getVector3fMap() = open3d_cloud->points_[i].cast<float>();
}
return pcl_cloud;
};
template<>
std::shared_ptr<pcl::PointCloud<pcl::PointNormal>> to_pcl<pcl::PointNormal>( const std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud )
{
const uint32_t size = open3d_cloud->points_.size();
const Eigen::Vector3f zero = Eigen::Vector3f::Zero();
pcl::PointCloud<pcl::PointNormal>::Ptr pcl_cloud = pcl::make_shared<pcl::PointCloud<pcl::PointNormal>>();
pcl_cloud->width = size;
pcl_cloud->height = 1;
pcl_cloud->is_dense = false;
pcl_cloud->points.resize( size );
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
pcl_cloud->points[i].getVector3fMap() = open3d_cloud->points_[i].cast<float>();
const Eigen::Vector3f normal = ( open3d_cloud->HasNormals() ) ? open3d_cloud->normals_[i].cast<float>() : zero;
std::copy( normal.data(), normal.data() + normal.size(), pcl_cloud->points[i].normal );
}
return pcl_cloud;
};
template<>
std::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> to_pcl<pcl::PointXYZRGB>( const std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud )
{
const uint32_t size = open3d_cloud->points_.size();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
pcl_cloud->width = size;
pcl_cloud->height = 1;
pcl_cloud->is_dense = false;
pcl_cloud->points.resize( size );
if( open3d_cloud->HasColors() ){
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
pcl_cloud->points[i].getVector3fMap() = open3d_cloud->points_[i].cast<float>();
const auto color = ( open3d_cloud->colors_[i] * 255.0 ).cast<uint32_t>();
uint32_t rgb = color[0] << 16 | color[1] << 8 | color[2];
pcl_cloud->points[i].rgb = *reinterpret_cast<float*>( &rgb );
}
}
else{
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
pcl_cloud->points[i].getVector3fMap() = open3d_cloud->points_[i].cast<float>();
uint32_t rgb = 0x000000;
pcl_cloud->points[i].rgb = *reinterpret_cast<float*>( &rgb );
}
}
return pcl_cloud;
};
template<>
std::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA>> to_pcl<pcl::PointXYZRGBA>( const std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud )
{
const uint32_t size = open3d_cloud->points_.size();
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pcl_cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGBA>>();
pcl_cloud->width = size;
pcl_cloud->height = 1;
pcl_cloud->is_dense = false;
pcl_cloud->points.resize( size );
if( open3d_cloud->HasColors() ){
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
pcl_cloud->points[i].getVector3fMap() = open3d_cloud->points_[i].cast<float>();
const auto color = ( open3d_cloud->colors_[i] * 255.0 ).cast<uint32_t>();
pcl_cloud->points[i].rgba = 0xff000000 | color[0] << 16 | color[1] << 8 | color[2];
}
}
else{
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
pcl_cloud->points[i].getVector3fMap() = open3d_cloud->points_[i].cast<float>();
pcl_cloud->points[i].rgba = 0xff000000;
}
}
return pcl_cloud;
};
template<>
std::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBNormal>> to_pcl<pcl::PointXYZRGBNormal>( const std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud )
{
const uint32_t size = open3d_cloud->points_.size();
const Eigen::Vector3f zero = Eigen::Vector3f::Zero();
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGBNormal>>();
pcl_cloud->width = size;
pcl_cloud->height = 1;
pcl_cloud->is_dense = false;
pcl_cloud->points.resize( size );
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
pcl_cloud->points[i].getVector3fMap() = open3d_cloud->points_[i].cast<float>();
const auto color = ( open3d_cloud->colors_[i] * 255.0 ).cast<uint32_t>();
uint32_t rgb = ( open3d_cloud->HasColors() ) ? color[0] << 16 | color[1] << 8 | color[2] : 0x000000;
pcl_cloud->points[i].rgb = *reinterpret_cast<float*>( &rgb );
const Eigen::Vector3f normal = ( open3d_cloud->HasNormals() ) ? open3d_cloud->normals_[i].cast<float>() : zero;
std::copy( normal.data(), normal.data() + normal.size(), pcl_cloud->points[i].normal );
}
return pcl_cloud;
};
template<typename T>
std::shared_ptr<open3d::geometry::PointCloud> to_open3d( const std::shared_ptr<pcl::PointCloud<T>> pcl_cloud )
{
std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud = std::make_shared<open3d::geometry::PointCloud>();
return open3d_cloud;
};
template<>
std::shared_ptr<open3d::geometry::PointCloud> to_open3d<pcl::PointXYZ>( const std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> pcl_cloud )
{
const uint32_t size = pcl_cloud->size();
std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud = std::make_shared<open3d::geometry::PointCloud>();
open3d_cloud->points_.resize( size );
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
open3d_cloud->points_[i] = pcl_cloud->points[i].getVector3fMap().cast<double>();
}
return open3d_cloud;
};
template<>
std::shared_ptr<open3d::geometry::PointCloud> to_open3d<pcl::PointNormal>( const std::shared_ptr<pcl::PointCloud<pcl::PointNormal>> pcl_cloud )
{
const uint32_t size = pcl_cloud->size();
std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud = std::make_shared<open3d::geometry::PointCloud>();
open3d_cloud->points_.resize( size );
open3d_cloud->normals_.resize( size );
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
open3d_cloud->points_[i] = pcl_cloud->points[i].getVector3fMap().cast<double>();
open3d_cloud->normals_[i] = pcl_cloud->points[i].getNormalVector3fMap().cast<double>();
}
return open3d_cloud;
};
template<>
std::shared_ptr<open3d::geometry::PointCloud> to_open3d<pcl::PointXYZRGB>( const std::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> pcl_cloud )
{
const uint32_t size = pcl_cloud->size();
std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud = std::make_shared<open3d::geometry::PointCloud>();
open3d_cloud->points_.resize( size );
open3d_cloud->colors_.resize( size );
constexpr double normal = 1.0 / 255.0;
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
open3d_cloud->points_[i] = pcl_cloud->points[i].getVector3fMap().cast<double>();
const uint32_t color = *reinterpret_cast<uint32_t*>( &pcl_cloud->points[i].rgb );
open3d_cloud->colors_[i] = Eigen::Vector3d( ( color >> 16 ) & 0x0000ff, ( color >> 8 ) & 0x0000ff, color & 0x0000ff ) * normal;
}
return open3d_cloud;
};
template<>
std::shared_ptr<open3d::geometry::PointCloud> to_open3d<pcl::PointXYZRGBA>( const std::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA>> pcl_cloud )
{
const uint32_t size = pcl_cloud->size();
std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud = std::make_shared<open3d::geometry::PointCloud>();
open3d_cloud->points_.resize( size );
open3d_cloud->colors_.resize( size );
constexpr double normal = 1.0 / 255.0;
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
open3d_cloud->points_[i] = pcl_cloud->points[i].getVector3fMap().cast<double>();
const uint32_t color = pcl_cloud->points[i].rgba;
open3d_cloud->colors_[i] = Eigen::Vector3d( ( color >> 16 ) & 0x000000ff, ( color >> 8 ) & 0x000000ff, color & 0x000000ff ) * normal;
}
return open3d_cloud;
};
template<>
std::shared_ptr<open3d::geometry::PointCloud> to_open3d<pcl::PointXYZRGBNormal>( const std::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBNormal>> pcl_cloud )
{
const uint32_t size = pcl_cloud->size();
std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud = std::make_shared<open3d::geometry::PointCloud>();
open3d_cloud->points_.resize( size );
open3d_cloud->normals_.resize( size );
open3d_cloud->colors_.resize( size );
constexpr double normal = 1.0 / 255.0;
#pragma omp parallel for
for( int32_t i = 0; i < size; i++ ){
open3d_cloud->points_[i] = pcl_cloud->points[i].getVector3fMap().cast<double>();
open3d_cloud->normals_[i] = pcl_cloud->points[i].getNormalVector3fMap().cast<double>();
const uint32_t color = *reinterpret_cast<uint32_t*>( &pcl_cloud->points[i].rgb );
open3d_cloud->colors_[i] = Eigen::Vector3d( ( color >> 16 ) & 0x0000ff, ( color >> 8 ) & 0x0000ff, color & 0x0000ff ) * normal;
}
return open3d_cloud;
};
}
#endif // __CONVERT__
#define NOMINMAX
#include <iostream>
#include <stdexcept>
#include <Open3D/Open3D.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include "convert.h"
// Point Type
// pcl::PointXYZ, pcl::PointNormal, pcl::PointXYZRGB, pcl::PointXYZRGBA, pcl::PointXYZRGBNormal
typedef pcl::PointXYZRGB PointType;
template<typename T>
void set_color( std::shared_ptr<pcl::PointCloud<T>> cloud )
{
}
template<>
void set_color<pcl::PointXYZ>( std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> cloud )
{
}
template<>
void set_color<pcl::PointXYZRGB>( std::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud )
{
for( int32_t i = 0; i < cloud->size(); i++ ){
uint32_t rgb = 0 << 16 | 0 << 8 | 255;
cloud->points[i].rgb = *reinterpret_cast<float*>( &rgb );
}
}
template<>
void set_color<pcl::PointXYZRGBA>( std::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA>> cloud )
{
for( int32_t i = 0; i < cloud->size(); i++ ){
uint32_t rgba = 0xff000000 | 0 << 16 | 0 << 8 | 255;
cloud->points[i].rgba = rgba;
}
}
void set_color( std::shared_ptr<open3d::geometry::PointCloud> cloud )
{
cloud->PaintUniformColor( Eigen::Vector3d( 0.0, 0.0, 1.0 ) );
}
int main( int argc, char* argv[] )
{
try{
// Convert Point Cloud To PCL From Open3D
{
// Read Point Cloud
bool result = false;
std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud = std::make_shared<open3d::geometry::PointCloud>();
result = open3d::io::ReadPointCloud( "../bunny.pcd", *open3d_cloud );
if( !result ){
throw std::runtime_error( "failed to read point cloud!" );
}
// Set Color
set_color( open3d_cloud );
// Cpnvert Point Cloud
pcl::PointCloud<PointType>::Ptr pcl_cloud = convert::to_pcl<PointType>( open3d_cloud );
// Show Point Cloud
pcl::visualization::PointCloudColorHandler<PointType>::Ptr handler;
//handler = pcl::make_shared<pcl::visualization::PointCloudColorHandlerCustom<PointType>>( pcl_cloud, 255.0, 255.0, 255.0 ); // pcl::PointXYZ
handler = pcl::make_shared<pcl::visualization::PointCloudColorHandlerRGBField<PointType>>( pcl_cloud ); // pcl::PointXYZRGB
//handler = pcl::make_shared<pcl::visualization::PointCloudColorHandlerRGBAField<PointType>>( pcl_cloud ); // pcl::PointXYZRGBA
pcl::visualization::PCLVisualizer::Ptr pcl_viewer = pcl::make_shared<pcl::visualization::PCLVisualizer>();
pcl_viewer->setWindowName( "PCL" );
pcl_viewer->addPointCloud( pcl_cloud, *handler, "cloud" );
while( !pcl_viewer->wasStopped() ){
pcl_viewer->spinOnce();
}
pcl_viewer->close();
}
// Convert Point Cloud To Open3D From PCL
{
// Read Point Cloud
int32_t result = -1;
pcl::PointCloud<PointType>::Ptr pcl_cloud = pcl::make_shared <pcl::PointCloud<PointType>>();
result = pcl::io::loadPCDFile( "../bunny.pcd", *pcl_cloud );
if( result ){
throw std::runtime_error( "failed to read point cloud!" );
}
// Set Color
set_color( pcl_cloud );
// Convert Point Cloud
std::shared_ptr<open3d::geometry::PointCloud> open3d_cloud = convert::to_open3d<PointType>( pcl_cloud );
// Show Point Cloud
open3d::visualization::Visualizer open3d_viewer;
open3d_viewer.CreateVisualizerWindow( "Open3D", 1280, 720 );
open3d_viewer.AddGeometry( { open3d_cloud } );
open3d_viewer.BuildUtilities();
while( open3d_viewer.PollEvents() ){}
open3d_viewer.Close();
open3d_viewer.DestroyVisualizerWindow();
}
}
catch( const std::runtime_error& error ){
std::cout << error.what() << std::endl;
return -1;
}
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.