#include?

using?namespace?std;
#include??//PCL的PCD格式文件的輸入輸出頭文件

#include??//PCL對各種格式的點的支持頭文件

#include?//點云查看窗口頭文件
#include?
#include?
#include
#include?
#include
#include?
#include
#include?
using?namespace?pcl;

bool?estimateGroundPlane（pcl::PointCloud::Ptr?&in_cloud?pcl::PointCloud::Ptr?&out_cloud
?????????????????????????const?float?in_distance_thre）;

typedef?pcl::PointXYZRGBA?PointT;
typedef?pcl::PointCloud?PointCloudT;

pcl::PointCloud::Ptr?clicked_points_3d（new?pcl::PointCloud）;
int?num?=?0;
pcl::PointCloud::Ptr?cloud（new?pcl::PointCloud）;?//?創建點云（指針）
pcl::PointCloud::Ptr?cloud_final（new?pcl::PointCloud）;?//?創建點云（指針）
boost::shared_ptr?viewer（new?pcl::visualization::PCLVisualizer（“viewer“））;

//?Mutex:?//
boost::mutex?cloud_mutex;

struct?callback_args{
????//?structure?used?to?pass?arguments?to?the?callback?function
????PointCloudT::Ptr?clicked_points_3d;
????pcl::visualization::PCLVisualizer::Ptr?viewerPtr;
};

Eigen::Matrix4f?CreateRotateMatrix（Eigen::Vector3f?beforeEigen::Vector3f?after）
{
????before.normalize（）;
????after.normalize（）;

????float?angle?=?acos（before.dot（after））;
????Eigen::Vector3f?p_rotate?=before.cross（after）;
????p_rotate.normalize（）;

????Eigen::Matrix4f?rotationMatrix?=?Eigen::Matrix4f::Identity（）;
????rotationMatrix（0?0）?=?cos（angle）?+?p_rotate[0]?*?p_rotate[0]?*?（1?-?cos（angle））;
????rotationMatrix（0?1）?=?p_rotate[0]?*?p_rotate[1]?*?（1?-?cos（angle）?-?p_rotate[2]?*?sin（angle））;//這里跟公式比多了一個括號，但是看實驗結果它是對的。
????rotationMatrix（0?2）?=?p_rotate[1]?*?sin（angle）?+?p_rotate[0]?*?p_rotate[2]?*?（1?-?cos（angle））;


????rotationMatrix（1?0）?=?p_rotate[2]?*?sin（angle）?+?p_rotate[0]?*?p_rotate[1]?*?（1?-?cos（angle））;
????rotationMatrix（1?1）?=?cos（angle）?+?p_rotate[1]?*?p_rotate[1]?*?（1?-?cos（angle））;
????rotationMatrix（1?2）?=?-p_rotate[0]?*?sin（angle）?+?p_rotate[1]?*?p_rotate[2]?*?（1?-?cos（angle））;


????rotationMatrix（2?0）?=?-p_rotate[1]?*?sin（angle）?+p_rotate[0]?*?p_rotate[2]?*?（1?-?cos（angle））;
????rotationMatrix（2?1）?=?p_rotate[0]?*?sin（angle）?+?p_rotate[1]?*?p_rotate[2]?*?（1?-?cos（angle））;
????rotationMatrix（2?2）?=?cos（angle）?+?p_rotate[2]?*?p_rotate[2]?*?（1?-?cos（angle））;

????return?rotationMatrix;
}

void?AreaPick_callback（const?pcl::visualization::AreaPickingEvent&?event?void*?args）
{
????std::vector?indices;
????if?（event.getPointsIndices（indices）==-1）
????????return;

????for?（int?i?=?0;?i?????{
????????clicked_points_3d->points.push_back（cloud->points.at（indices[i]））;
????}
????pcl::PointCloud