Last active
September 4, 2019 14:37
-
-
Save UnaNancyOwen/2ac64d344a27e0c66f802545aefe4ca9 to your computer and use it in GitHub Desktop.
Drawing Point Cloud retrieve from Intel RealSense Depth Camera (D415/D435)
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
cmake_minimum_required( VERSION 3.6 ) | |
# Require C++14 (or later) | |
set( CMAKE_CXX_STANDARD 14 ) | |
set( CMAKE_CXX_STANDARD_REQUIRED ON ) | |
set( CMAKE_CXX_EXTENSIONS OFF ) | |
project( sample ) | |
add_executable( sample main.cpp ) | |
set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "sample" ) | |
# Find Packages | |
find_package( PCL 1.9 REQUIRED ) | |
if( PCL_FOUND ) | |
# Additional Include Directories | |
include_directories( ${PCL_INCLUDE_DIRS} ) | |
# Preprocessor Definitions | |
add_definitions( ${PCL_DEFINITIONS} ) | |
# Additional Library Directories | |
link_directories( ${PCL_LIBRARY_DIRS} ) | |
# Additional Dependencies | |
target_link_libraries( sample ${PCL_LIBRARIES} ) | |
endif() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <pcl/point_cloud.h> | |
#include <pcl/point_types.h> | |
#include <pcl/io/real_sense_2_grabber.h> | |
#include <pcl/visualization/pcl_visualizer.h> | |
// Point Type | |
// pcl::PointXYZ, pcl::PointXYZI, pcl::PointXYZRGB, pcl::PointXYZRGBA | |
typedef pcl::PointXYZRGB PointType; | |
int main( int argc, char* argv[] ) | |
{ | |
// PCL Visualizer | |
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = boost::make_shared<pcl::visualization::PCLVisualizer>( "Point Cloud Viewer" ); | |
viewer->addCoordinateSystem( 0.1 ); | |
viewer->setCameraPosition( 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0 ); | |
// Point Cloud Color Hndler | |
boost::shared_ptr<pcl::visualization::PointCloudColorHandler<PointType>> handler; | |
const std::type_info& type = typeid( PointType ); | |
if( type == typeid( pcl::PointXYZ ) ){ | |
std::vector<double> color = { 255.0, 255.0, 255.0 }; | |
handler = boost::make_shared<pcl::visualization::PointCloudColorHandlerCustom<PointType>>( color[0], color[1], color[2] ); | |
} | |
else if( type == typeid( pcl::PointXYZI ) ){ | |
handler = boost::make_shared<pcl::visualization::PointCloudColorHandlerGenericField<PointType>>( "intensity" ); | |
} | |
else if( type == typeid( pcl::PointXYZRGB ) ){ | |
handler = boost::make_shared<pcl::visualization::PointCloudColorHandlerRGBField<PointType>>(); | |
} | |
else if( type == typeid( pcl::PointXYZRGBA ) ){ | |
handler = boost::make_shared<pcl::visualization::PointCloudColorHandlerRGBAField<PointType>>(); | |
} | |
else{ | |
throw std::runtime_error( "This PointType is unsupported." ); | |
} | |
// Point Cloud | |
pcl::PointCloud<PointType>::ConstPtr cloud; | |
// Retrieved Point Cloud Callback Function | |
boost::mutex mutex; | |
boost::function<void( const pcl::PointCloud<PointType>::ConstPtr& )> function = | |
[&cloud, &mutex]( const pcl::PointCloud<PointType>::ConstPtr& ptr ){ | |
boost::mutex::scoped_lock lock( mutex ); | |
/* Point Cloud Processing */ | |
cloud = ptr->makeShared(); | |
}; | |
// RealSense2Grabber | |
boost::shared_ptr<pcl::RealSense2Grabber> grabber = boost::make_shared<pcl::RealSense2Grabber>(); | |
//boost::shared_ptr<pcl::RealSense2Grabber> grabber = boost::make_shared<pcl::RealSense2Grabber>( "746112061315" ); // specific device (serial number) | |
//boost::shared_ptr<pcl::RealSense2Grabber> grabber = boost::make_shared<pcl::RealSense2Grabber>( "../file.bag" ); // bag file | |
// Set Frame Size and FPS | |
//grabber->setDeviceOptions( 640, 480, 30 ); | |
// Register Callback Function | |
boost::signals2::connection connection = grabber->registerCallback( function ); | |
// Start Grabber | |
grabber->start(); | |
while( !viewer->wasStopped() ){ | |
// Update Viewer | |
viewer->spinOnce(); | |
boost::mutex::scoped_try_lock lock( mutex ); | |
if( lock.owns_lock() && cloud ){ | |
// Update Point Cloud | |
handler->setInputCloud( cloud ); | |
if( !viewer->updatePointCloud( cloud, *handler, "cloud" ) ){ | |
viewer->addPointCloud( cloud, *handler, "cloud" ); | |
} | |
} | |
} | |
// Stop Grabber | |
grabber->stop(); | |
// Disconnect Callback Function | |
if( connection.connected() ){ | |
connection.disconnect(); | |
} | |
return 0; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
where is the <pcl/io/real_sense_2_grabber.h> file? Thanks.