PCL經典程式碼賞析六：深度影象及關鍵點提取
阿新 • • 發佈：2019-02-16
##· 說明
##· 目錄索引
- PCL 深度影象
- PCL 深度影象提取邊界
- PCL 關鍵點
- NARF(Normal Aligned Radial Feature) 關鍵點
##· PCL 深度影象
在 PCL 中深度影象與點雲最主要的區別在於其近鄰的檢索方式的不同，並且可以互相轉換
深度影象，也稱為距離影像（Range Image），是指將從影象採集器到場景中各點的距離值作為畫素值的影象，它直接反應了景物可見表面的幾何形狀，利用它可以很方便的解決3D目標描述中的許多問題，深度影象經過點雲變換可以計算為點雲資料，有規則及有必要資訊的點雲資料可以反算為深度影象資料。
點雲，當一束鐳射照射到物體表面時，所反射的鐳射會攜帶方位、距離等資訊。若將鐳射束按照某種軌跡進行掃描，便會邊掃描邊記錄到反射的鐳射點資訊，由 於掃描極為精細，則能夠得到大量的鐳射點，因而就可形成鐳射點雲。點雲格式有
.las ;*.pcd; .txt等rangeimage是來自感測器一個特定角度拍攝的一個三維場景獲取的有規則的有焦距等基本資訊的深度圖
深度影象的畫素值代表從感測器到物體的距離或者深度值
目前深度影象的獲取方法有：
- 計算機立體視覺成像
- 鐳射雷達深度成像法
- 莫爾條紋法
- 結構光法等
針對深度影象的研究重點主要集中在以下幾個方面：
- 深度影象的分割技術
- 深度影象的邊緣檢測技術
- 基於不同視點的多幅深度影象的配準技術
- 基於深度資料的三維重建技術
- 基於三維深度影象的三維目標識別技術
- 深度影象的多解析度建模和幾何壓縮技術等
不同視角獲得深度影象的過程：
程式碼片段一：獲得深度影象
//如何獲得深度影象 #include <pcl/range_image/range_image.h> //深度影象的標頭檔案 int main (int argc, char** argv) { pcl::PointCloud<pcl::PointXYZ> pointCloud; //定義點雲的物件 // 迴圈產生點雲的資料 for (float y=-0.5f; y<=0.5f; y+=0.01f) { for (float z=-0.5f; z<=0.5f; z+=0.01f) { pcl::PointXYZ point; point.x = 2.0f - y; point.y = y; point.z = z; pointCloud.points.push_back(point); //迴圈新增點資料到點雲物件 } } pointCloud.width = (uint32_t) pointCloud.points.size(); pointCloud.height = 1; //設定點雲物件的頭資訊 //實現一個呈矩形形狀的點雲 // We now want to create a range image from the above point cloud, with a 1deg angular resolution //angular_resolution為模擬的深度感測器的角度解析度，即深度影象中一個畫素對應的角度大小 float angularResolution = (float) ( 1.0f * (M_PI/180.0f)); // 1.0 degree in radians //max_angle_width為模擬的深度感測器的水平最大采樣角度， float maxAngleWidth = (float) (360.0f * (M_PI/180.0f)); // 360.0 degree in radians //max_angle_height為模擬感測器的垂直方向最大采樣角度 都轉為弧度 float maxAngleHeight = (float) (180.0f * (M_PI/180.0f)); // 180.0 degree in radians //感測器的採集位置 Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f); //深度影象遵循座標系統 pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME; float noiseLevel=0.00; //noise_level獲取深度影象深度時，近鄰點對查詢點距離值的影響水平 float minRange = 0.0f; //min_range設定最小的獲取距離，小於最小獲取距離的位置為感測器的盲區 int borderSize = 1; //border_size獲得深度影象的邊緣的寬度 pcl::RangeImage rangeImage; rangeImage.createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight, sensorPose, coordinate_frame, noiseLevel, minRange, borderSize); std::cout << rangeImage << "\n"; }
程式碼片段二：有序點雲資料深度影象
#include <iostream> #include <boost/thread/thread.hpp> #include <pcl/common/common_headers.h> #include <pcl/range_image/range_image.h> //關於深度影象的標頭檔案 #include <pcl/io/pcd_io.h> #include <pcl/visualization/range_image_visualizer.h> //深度圖視覺化的標頭檔案 #include <pcl/visualization/pcl_visualizer.h> //PCL視覺化的標頭檔案 #include <pcl/console/parse.h> typedef pcl::PointXYZ PointType; //引數 float angular_resolution_x = 0.5f,//angular_resolution為模擬的深度感測器的角度解析度，即深度影象中一個畫素對應的角度大小 angular_resolution_y = angular_resolution_x; pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;//深度影象遵循座標系統 bool live_update = false; //命令幫助提示 void printUsage (const char* progName) { std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n" << "Options:\n" << "-------------------------------------------\n" << "-rx <float> angular resolution in degrees (default "<<angular_resolution_x<<")\n" << "-ry <float> angular resolution in degrees (default "<<angular_resolution_y<<")\n" << "-c <int> coordinate frame (default "<< (int)coordinate_frame<<")\n" << "-l live update - update the range image according to the selected view in the 3D viewer.\n" << "-h this help\n" << "\n\n"; } 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.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2], look_at_vector[0], look_at_vector[1], look_at_vector[2], up_vector[0], up_vector[1], up_vector[2]); } //主函式 int main (int argc, char** argv) { //輸入命令分析 if (pcl::console::find_argument (argc, argv, "-h") >= 0) { printUsage (argv[0]); return 0; } if (pcl::console::find_argument (argc, argv, "-l") >= 0) { live_update = true; std::cout << "Live update is on.\n"; } if (pcl::console::parse (argc, argv, "-rx", angular_resolution_x) >= 0) std::cout << "Setting angular resolution in x-direction to "<<angular_resolution_x<<"deg.\n"; if (pcl::console::parse (argc, argv, "-ry", angular_resolution_y) >= 0) std::cout << "Setting angular resolution in y-direction to "<<angular_resolution_y<<"deg.\n"; int tmp_coordinate_frame; if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0) { coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame); std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n"; } angular_resolution_x = pcl::deg2rad (angular_resolution_x); angular_resolution_y = pcl::deg2rad (angular_resolution_y); //讀取點雲PCD檔案 如果沒有輸入PCD檔案就生成一個點雲 pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>); pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr; Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ()); //申明感測器的位置是一個4*4的仿射變換 std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd"); if (!pcd_filename_indices.empty ()) { std::string filename = argv[pcd_filename_indices[0]]; if (pcl::io::loadPCDFile (filename, point_cloud) == -1) { std::cout << "Was not able to open file \""<<filename<<"\".\n"; printUsage (argv[0]); return 0; } //給感測器的位姿賦值 就是獲取點雲的感測器的的平移與旋轉的向量 scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0], point_cloud.sensor_origin_[1], point_cloud.sensor_origin_[2])) * Eigen::Affine3f (point_cloud.sensor_orientation_); } else { //如果沒有給點雲，則我們要自己生成點雲 std::cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n"; 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; point_cloud.points.push_back (point); } } point_cloud.width = (int) point_cloud.points.size (); point_cloud.height = 1; } // -----從建立的點雲中獲取深度圖--// //設定基本引數 float noise_level = 0.0; 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（）引數的解釋 （涉及的角度都為弧度為單位） ： point_cloud為建立深度影象所需要的點雲 angular_resolution_x深度感測器X方向的角度解析度 angular_resolution_y深度感測器Y方向的角度解析度 pcl::deg2rad (360.0f)深度感測器的水平最大采樣角度 pcl::deg2rad (180.0f)垂直最大采樣角度 scene_sensor_pose設定的模擬感測器的位姿是一個仿射變換矩陣，預設為4*4的單位矩陣變換 coordinate_frame定義按照那種座標系統的習慣 預設為CAMERA_FRAME noise_level 獲取深度影象深度時，鄰近點對查詢點距離值的影響水平 min_range 設定最小的獲取距離，小於最小的獲取距離的位置為感測器的盲區 border_size 設定獲取深度影象邊緣的寬度 預設為0 */ range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size); //視覺化點雲 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.addCoordinateSystem (1.0f, "global"); //PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150); //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud"); viewer.initCameraParameters (); //range_image.getTransformationToWorldSystem ()的作用是獲取從深度影象座標系統（應該就是感測器的座標）轉換為世界座標系統的轉換矩陣 setViewerPose(viewer, range_image.getTransformationToWorldSystem ()); //設定視點的位置 //視覺化深度圖 pcl::visualization::RangeImageVisualizer range_image_widget ("Range image"); range_image_widget.showRangeImage (range_image); while (!viewer.wasStopped ()) { range_image_widget.spinOnce (); viewer.spinOnce (); pcl_sleep (0.01); if (live_update) { //如果選擇的是——l的引數說明就是要根據自己選擇的視點來建立深度圖。 // live update - update the range image according to the selected view in the 3D viewer. scene_sensor_pose = viewer.getViewerPose(); range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f), scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noise_level, min_range, border_size); range_image_widget.showRangeImage (range_image); } } }
##· PCL 深度影象提取邊界
深度影象邊界：計從前景跨越到背景的位置定義為邊界。具體有：
------ 物體邊界：這是物體的最外層和陰影邊界的可見點集
------ 陰影邊界：毗鄰與遮擋的背景上的點集
------ Veil點集：在被遮擋物邊界和陰影邊界之間的內插點，它們是有鐳射雷達獲取的3D距離資料中的典型資料型別
這三類資料及深度影象的邊界如圖：
建立深度影象並使其視覺化，提取邊界資訊很重要的一點就是區分深度影象中當前視點不可見點幾何和應該可見但處於感測器獲取距離範圍之外的點集 ，後者可以標記為典型邊界，然而當前視點不可見點則不能成為邊界。因此，如果後者的測量值存在，則提供那些超出感測器距離獲取範圍之外的資料對於邊界的提取是非常重要的。
#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/console/parse.h>
typedef pcl::PointXYZ PointType;
// --------------------
// -----Parameters-----
// --------------------
float angular_resolution = 0.5f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;
// --------------
// -----Help-----
// --------------
void
printUsage (const char* progName)
{
std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
<< "Options:\n"
<< "-------------------------------------------\n"
<< "-r <float> angular resolution in degrees (default "<<angular_resolution<<")\n"
<< "-c <int> coordinate frame (default "<< (int)coordinate_frame<<")\n"
<< "-m Treat all unseen points to max range\n"
<< "-h this help\n"
<< "\n\n";
}
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::find_argument (argc, argv, "-m") >= 0)
{
setUnseenToMaxRange = true;
cout << "Setting unseen values in range image to maximum range readings.\n";
}
int tmp_coordinate_frame;
if (pcl::console::parse (argc, argv, "-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 (argc, argv, "-r", angular_resolution) >= 0)
cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
angular_resolution = pcl::deg2rad (angular_resolution);
// ------------------------------------------------------------------
// -----Read pcd file or create example point cloud if not given-----
// ------------------------------------------------------------------
pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ()); //感測器的位置
std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
if (!pcd_filename_indices.empty ())
{
std::string filename = argv[pcd_filename_indices[0]];
if (pcl::io::loadPCDFile (filename, point_cloud) == -1) //開啟檔案
{
cout << "Was not able to open file \""<<filename<<"\".\n";
printUsage (argv[0]);
return 0;
}
scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
point_cloud.sensor_origin_[1],
point_cloud.sensor_origin_[2])) *
Eigen::Affine3f (point_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";
}
else
{
cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
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;
point_cloud.points.push_back (point);
}
}
point_cloud.width = (int) point_cloud.points.size (); point_cloud.height = 1;
}
// -----------------------------------------------
// -----Create RangeImage from the PointCloud-----
// -----------------------------------------------
float noise_level = 0.0; //各種引數的設定
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 (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
range_image.integrateFarRanges (far_ranges);
if (setUnseenToMaxRange)
range_image.setUnseenToMaxRange ();
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
pcl::visualization::PCLVisualizer viewer ("3D Viewer"); //建立視口
viewer.setBackgroundColor (1, 1, 1); //設定背景顏色
viewer.addCoordinateSystem (1.0f); //設定座標系
pcl::visualization::PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 0, 0, 0);
viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud"); //新增點雲
//PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 150, 150, 150);
//viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
//viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 2, "range image");
// -------------------------
// -----Extract borders提取邊界的部分-----
// -------------------------
pcl::RangeImageBorderExtractor border_extractor (&range_image);
pcl::PointCloud<pcl::BorderDescription> border_descriptions;
border_extractor.compute (border_descriptions); //提取邊界計算描述子
// -------------------------------------------------------
// -----Show points in 3D viewer在3D 視口中顯示點雲-----
// ----------------------------------------------------
pcl::PointCloud<pcl::PointWithRange>::Ptr border_points_ptr(new pcl::PointCloud<pcl::PointWithRange>), //物體邊界
veil_points_ptr(new pcl::PointCloud<pcl::PointWithRange>), //veil邊界
shadow_points_ptr(new pcl::PointCloud<pcl::PointWithRange>); //陰影邊界
pcl::PointCloud<pcl::PointWithRange>& border_points = *border_points_ptr,
& veil_points = * veil_points_ptr,
& shadow_points = *shadow_points_ptr;
for (int y=0; y< (int)range_image.height; ++y)
{
for (int x=0; x< (int)range_image.width; ++x)
{
if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER])
border_points.points.push_back (range_image.points[y*range_image.width + x]);
if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT])
veil_points.points.push_back (range_image.points[y*range_image.width + x]);
if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER])
shadow_points.points.push_back (range_image.points[y*range_image.width + x]);
}
}
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> border_points_color_handler (border_points_ptr, 0, 255, 0);
viewer.addPointCloud<pcl::PointWithRange> (border_points_ptr, border_points_color_handler, "border points");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "border points");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> veil_points_color_handler (veil_points_ptr, 255, 0, 0);
viewer.addPointCloud<pcl::PointWithRange> (veil_points_ptr, veil_points_color_handler, "veil points");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "veil points");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> shadow_points_color_handler (shadow_points_ptr, 0, 255, 255);
viewer.addPointCloud<pcl::PointWithRange> (shadow_points_ptr, shadow_points_color_handler, "shadow points");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "shadow points");
//-------------------------------------
// -----Show points on range image-----
// ------------------------------------
pcl::visualization::RangeImageVisualizer* range_image_borders_widget = NULL;
range_image_borders_widget =
pcl::visualization::RangeImageVisualizer::getRangeImageBordersWidget (range_image, -std::numeric_limits<float>::infinity (), std::numeric_limits<float>::infinity (), false,
border_descriptions, "Range image with borders");
// -------------------------------------
//--------------------
// -----Main loop-----
//--------------------
while (!viewer.wasStopped ())
{
range_image_borders_widget->spinOnce ();
viewer.spinOnce ();
pcl_sleep(0.01);
}
}
檢測出的邊界用綠色較大的點表示，其他的點用預設的普通的大小點來表示
因為有人問我為什麼使用其他的PCD檔案來執行這個程式的時候會提示 Far ranges file far_ranges_filename does not exists ？這是因為在深度感測器得帶深度影象並可視化影象的時候，我們都知道感測器的測量距離受硬體的限制，所以在這裡就是要定義感測器看不到的距離，所以當我們自己使用kinect獲取深度影象執行這個程式的時候直接使用命令：
./range_image_border_extraction -m out0.pcd 使用-m的原因是要設定感測器看不見的位置 Setting unseen values in range image to maximum range readings
##· PCL 關鍵點
關鍵點也稱為興趣點，它是2D影象或是3D點雲或者曲面模型上，可以通過定義檢測標準來獲取的具有穩定性、區別性的點集，從技術上來說，關鍵點的數量相比於原始點雲或影象的資料量減小很多、關鍵點與區域性特徵描述子結合在一起組成關鍵點描述子，它常用來表示原始資料，而且不失代表性和描述性，從而加快了後續的識別、追蹤等的速度。
##· NARF(Normal Aligned Radial Feature) 關鍵點
是為了從深度影象中識別物體而提出的
- 對NARF關鍵點的提取過程有以下要求：
- 提取的過程考慮邊緣以及物體表面變化資訊在內
- 在不同視角關鍵點可以被重複探測
- 關鍵點所在位置有足夠的支援區域，可以計算描述子和進行唯一的估計法向量
-
其對應的探測步驟如下：
- 遍歷每個深度影象點，通過尋找在近鄰區域有深度變化的位置進行邊緣檢測
- 遍歷每個深度影象點，根據近鄰區域的表面變化決定一測度表面變化的係數，及變化的主方向
- 根據第二步找到的主方向計算興趣點特徵，表徵該方向和其他方向的不同，以及該處表面的變化情況（穩定性）
- 對興趣值進行平滑濾波
- 進行無最大值壓縮找到的最終關鍵點，即為NARF關鍵點
-
關於NARF的更為具體的描述請檢視**這篇部落格**
#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>
#include <pcl/features/narf_descriptor.h>
#include <pcl/console/parse.h>
typedef pcl::PointXYZ PointType;
// --------------------
// -----Parameters-----
// --------------------
float angular_resolution = 0.5f; ////angular_resolution為模擬的深度感測器的角度解析度，即深度影象中一個畫素對應的角度大小
float support_size = 0.2f; //點雲大小的設定
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME; //設定座標系
bool setUnseenToMaxRange = false;
bool rotation_invariant = true;
// --------------
// -----Help-----
// --------------
void
printUsage (const char* progName)
{
std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
<< "Options:\n"
<< "-------------------------------------------\n"
<< "-r <float> angular resolution in degrees (default "<<angular_resolution<<")\n"
<< "-c <int> coordinate frame (default "<< (int)coordinate_frame<<")\n"
<< "-m Treat all unseen points to max range\n"
<< "-s <float> support size for the interest points (diameter of the used sphere - "
"default "<<support_size<<")\n"
<< "-o <0/1> switch rotational invariant version of the feature on/off"
<< " (default "<< (int)rotation_invariant<<")\n"
<< "-h this help\n"
<< "\n\n";
}
void
setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose) //設定視口的位姿
{
Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0); //視口的原點pos_vector
Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector; //旋轉+平移look_at_vector
Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0); //up_vector
viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2], //設定照相機的位姿
look_at_vector[0], look_at_vector[1], look_at_vector[2],
up_vector[0], up_vector[1], up_vector[2]);
}
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::find_argument (argc, argv, "-m") >= 0)
{
setUnseenToMaxRange = true;
cout << "Setting unseen values in range image to maximum range readings.\n";
}
if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0)
cout << "Switching rotation invariant feature version "<< (rotation_invariant ? "on" : "off")<<".\n";
int tmp_coordinate_frame;
if (pcl::console::parse (argc, argv, "-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 (argc, argv, "-s", support_size) >= 0)
cout << "Setting support size to "<<support_size<<".\n";
if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
angular_resolution = pcl::deg2rad (angular_resolution);
// ------------------------------------------------------------------
// -----Read pcd file or create example point cloud if not given-----
// ------------------------------------------------------------------
pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
if (!pcd_filename_indices.empty ())
{
std::string filename = argv[pcd_filename_indices[0]];
if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
{
cerr << "Was not able to open file \""<<filename<<"\".\n";
printUsage (argv[0]);
return 0;
}
scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0], //場景感測器的位置
point_cloud.sensor_origin_[1],
point_cloud.sensor_origin_[2])) *
Eigen::Affine3f (point_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";
}
else
{
setUnseenToMaxRange = true;
cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
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;
point_cloud.points.push_back (point);
}
}
point_cloud.width = (int) point_cloud.points.size (); point_cloud.height = 1;
}
// -----------------------------------------------
// -----Create RangeImage from the PointCloud-----
// -----------------------------------------------
float noise_level = 0.0;
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 (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
range_image.integrateFarRanges (far_ranges);
if (setUnseenToMaxRange)
range_image.setUnseenToMaxRange ();
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
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.addCoordinateSystem (1.0f, "global");
//PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
viewer.initCameraParameters ();
setViewerPose (viewer, range_image.getTransformationToWorldSystem ());
// --------------------------
// -----Show range image-----
// --------------------------
pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
range_image_widget.showRangeImage (range_image);
/*********************************************************************************************************
建立RangeImageBorderExtractor物件，它是用來進行邊緣提取的，因為NARF的第一步就是需要探測出深度影象的邊緣，
*********************************************************************************************************/
// --------------------------------
// -----Extract NARF keypoints-----
// --------------------------------
pcl::RangeImageBorderExtractor range_image_border_extractor; //用來提取邊緣
pcl::NarfKeypoint narf_keypoint_detector; //用來檢測關鍵點
narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor); //
narf_keypoint_detector.setRangeImage (&range_image);
narf_keypoint_detector.getParameters ().support_size = support_size; //設定NARF的引數
pcl::PointCloud<int> keypoint_indices;
narf_keypoint_detector.compute (keypoint_indices);
std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";
// ----------------------------------------------
// -----Show keypoints in range image widget-----
// ----------------------------------------------
//for (size_t i=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);
// -------------------------------------
// -----Show keypoints in 3D viewer-----
// -------------------------------------
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");
// ------------------------------------------------------
// -----Extract NARF descriptors for interest points-----
// ------------------------------------------------------
std::vector<int> keypoint_indices2;
keypoint_indices2.resize (keypoint_indices.points.size ());
for (unsigned int i=0; i<keypoint_indices.size (); ++i) // This step is necessary to get the right vector type
keypoint_indices2[i]=keypoint_indices.points[i];
pcl::NarfDescriptor narf_descriptor (&range_image, &keypoint_indices2);
narf_descriptor.getParameters ().support_size = support_size;
narf_descriptor.getParameters ().rotation_invariant = rotation_invariant;
pcl::PointCloud<pcl::Narf36> narf_descriptors;
narf_descriptor.compute (narf_descriptors);
cout << "Extracted "<<narf_descriptors.size ()<<" descriptors for "
<<keypoint_indices.points.size ()<< " keypoints.\n";
//--------------------
// -----Main loop-----
//--------------------
while (!viewer.wasStopped ())
{
range_image_widget.spinOnce (); // process GUI events
viewer.spinOnce ();
pcl_sleep(0.01);
}
}