/*?\author?Bastian?Steder?*/

#include?
#include?
#include?
#include?
#include?
#include?
#include?
#include?
#include?

typedefpcl::PointXYZPointType;
//參數(shù)
floatangular_resolution=0.5f;
floatsupport_size=0.2f;
pcl::RangeImage::Coordinateframecoordinate_frame=pcl::RangeImage::CAMERA_frame;
boolsetUnseenToMaxRange=false;
//打印幫助
void
printUsage（constchar*progName）
{
std::cout<<“\n\nUsage:?“<\n\n“
<<“Options:\n“
<<“-------------------------------------------\n“
<<“-r????angular?resolution?in?degrees?（default?“<<<“-c??????coordinate?frame?（default?“<<（int）coordinate_frame<<“）\n“
<<“-m???????????Treat?all?unseen?points?as?maximum?range?readings\n“
<<“-s????support?size?for?the?interest?points?（diameter?of?the?used?sphere?-?“
<<“default?“<<<“-h???????????this?help\n“
<<“\n\n“;
}

void
setViewerPose（pcl::visualization::PCLVisualizer&viewerconstEigen::Affine3f&viewer_pose）
{
Eigen::Vector3fpos_vector=viewer_pose*Eigen::Vector3f（000）;
Eigen::Vector3flook_at_vector=viewer_pose.rotation（）*Eigen::Vector3f（001）+pos_vector;
Eigen::Vector3fup_vector=viewer_pose.rotation（）*Eigen::Vector3f（0-10）;
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（）;
}

//?-----Main-----
int
main（intargcchar**argv）
{
//解析命令行參數(shù)
if（pcl::console::find_argument（argcargv“-h“）>=0）
{
printUsage（argv[0]）;
return0;
}
if（pcl::console::find_argument（argcargv“-m“）>=0）
{
setUnseenToMaxRange=true;
cout<<“Setting?unseen?values?in?range?image?to?maximum?range?readings.\n“;
}
inttmp_coordinate_frame;
if（pcl::console::parse（argcargv“-c“tmp_coordinate_frame）>=0）
{
coordinate_frame=pcl::RangeImage::Coordinateframe（tmp_coordinate_frame）;
cout<<“Using?coordinate?frame?“<<（int）coordinate_frame<<“.\n“;
}
if（pcl::console::parse（argcargv“-s“support_size）>=0）
cout<<“Setting?support?size?to?“<if（pcl::console::parse（argcargv“-r“angular_resolution）>=0）
cout<<“Setting?angular?resolution?to?“<angular_resolution=pcl::deg2rad（angular_resolution）;
//讀取給定的pcd文件或者自行創(chuàng)建隨機點云
pcl::PointCloud::Ptrpoint_cloud_ptr（newpcl::PointCloud）;
pcl::PointCloud&point_cloud=*point_cloud_ptr;
pcl::PointCloudfar_ranges;
Eigen::Affine3fscene_sensor_pose（Eigen::