// NARF提取关键点.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include<iostream>
#include<pcl/point_types.h>
#include<pcl/range_image/range_image.h>
#include<boost/thread/thread.hpp>
#include<pcl/visualization/pcl_visualizer.h>
#include<pcl/visualization/range_image_visualizer.h>
#include<pcl/io/pcd_io.h>
#include<pcl/keypoints/narf_keypoint.h>
#include<pcl/console/parse.h>
#include<pcl/features/range_image_border_extractor.h>
#include<pcl/io/png_io.h>
typedef pcl::PointXYZ PointType;
float tangular_resolution=0.5f;
float support_size=0.5f;
pcl::RangeImage::CoordinateFrame coordinate_frame=pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange=true;
//void
//setViewerPose(pcl::visualization::PCLVisualizer&viewer,const Eigen::Affine3f&viewer_pose)
//{
//Eigen::Vector3f pos_vector=viewer_pose*Eigen::Vector3f(0,0,0);
//Eigen::Vector3f look_at_vector=viewer_pose.rotation()*Eigen::Vector3f(0,0,1)+pos_vector;
//Eigen::Vector3f up_vector=viewer_pose.rotation()*Eigen::Vector3f(0,-1,0);
//viewer.camera_.pos[0]=pos_vector[0];
//viewer.camera_.pos[1]=pos_vector[1];
//viewer.camera_.pos[2]=pos_vector[2];
//viewer.camera_.focal[0]=look_at_vector[0];
//viewer.camera_.focal[1]=look_at_vector[1];
//viewer.camera_.focal[2]=look_at_vector[2];
//viewer.camera_.view[0]=up_vector[0];
//viewer.camera_.view[1]=up_vector[1];
//viewer.camera_.view[2]=up_vector[2];
//viewer.updateCamera();
//}
int _tmain(int argc, _TCHAR* argv[])
{
pcl::PointCloud<PointType>::Ptr pointCloudPtr(new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType> &cloud=*pointCloudPtr;
pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
std::string filename="1.pcd";
if(setUnseenToMaxRange)
{
pcl::io::loadPCDFile(filename,cloud);
}
else
{
setUnseenToMaxRange=true;
for(float x=-0.5f;x<=0.5f;x+=0.01f)
{
for(float y=-0.5f;y<=0.5f;y+=0.01f)
{
PointType point;
point.x=x;
point.y=y;
point.z=2.0f-y;
cloud.points.push_back(point);
}
}
cloud.width=(int)cloud.points.size();
cloud.height=1;
}
std::cout<<cloud.points.size()<<std::endl;
scene_sensor_pose=Eigen::Affine3f(Eigen::Translation3f(cloud.sensor_origin_[0],
cloud.sensor_origin_[1],cloud.sensor_origin_[2]))*Eigen::Affine3f(cloud.sensor_orientation_);
std::string far_ranges_filename=pcl::getFilenameWithoutExtension(filename)+"_far_ranges.pcd";
if(pcl::io::loadPCDFile(far_ranges_filename.c_str(),far_ranges)==-1)
{
std::cout<<"Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
}
//从点云创建距离图像
float noise_level=0.0f;
float min_range=0.0f;
int border_size=1;
boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
pcl::RangeImage &range_image=*range_image_ptr;
range_image.createFromPointCloud(cloud,pcl::deg2rad(0.5f),pcl::deg2rad(360.0f),pcl::deg2rad(180.0f),
scene_sensor_pose,coordinate_frame,noise_level,min_range,border_size);
range_image.integrateFarRanges(far_ranges);
pcl::io::savePNGFile("1.png",range_image,"z");
if(setUnseenToMaxRange)
{
range_image.setUnseenToMaxRange();
//创建3D点云可视化窗口并显示点云
pcl::visualization::PCLVisualizer viewer("3D Viewer");
viewer.setBackgroundColor(1,1,1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler(range_image_ptr,0,0,0);
viewer.addPointCloud(range_image_ptr,range_image_color_handler,"range image");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"range image");
viewer.initCameraParameters();
//显示距离图
pcl::visualization::RangeImageVisualizer range_image_widget("Range image");
range_image_widget.showRangeImage(range_image);
//提取NARF关键点
pcl::RangeImageBorderExtractor range_image_border_extractor;
pcl::NarfKeypoint narf_keypoint_detector(&range_image_border_extractor);
narf_keypoint_detector.setRangeImage(&range_image);
narf_keypoint_detector.getParameters().support_size=support_size;
pcl::PointCloud<int>keypoint_indices;
narf_keypoint_detector.compute(keypoint_indices);
std::cout<<"Found "<<keypoint_indices.points.size()<<" key points.\n";
//在距离图像显示组件内显示关键点
//for (size_ti=0; i<keypoint_indices.points.size (); ++i)
//range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
//keypoint_indices.points[i]/range_image.width);
//在3D窗口中显示关键点
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>&keypoints=*keypoints_ptr;
keypoints.points.resize(keypoint_indices.points.size());
for(size_t i=0;i<keypoint_indices.points.size();++i)
{
keypoints.points[i].getVector3fMap()=range_image.points[keypoint_indices.points[i]].getVector3fMap();
}
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>keypoints_color_handler(keypoints_ptr,0,255,0);
viewer.addPointCloud<pcl::PointXYZ>(keypoints_ptr,keypoints_color_handler,"keypoints");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,7,"keypoints");
//主循环
while(!viewer.wasStopped())
{
range_image_widget.spinOnce();// process GUI events
viewer.spinOnce();
pcl_sleep(0.01);
}
}
getchar();
return 0;
}
评论0