第一章 点云数据采集
第二章 点云滤波
第二章 点云降采样
1.点云关键点是什么?
关键点也称为兴趣点,它是2D图像、3D点云或曲面模型上,可以通过定义检测标准来获取的具有稳定性、区别性的点集。
我们获得的数据量大,特别是几十万个以上的点云,里面有很多冗余数据,会导致处理起来比较耗时。
降采样是一种有效的减少数据、缩减计算量的方法。
2.关键点检测算法(本数据单位为mm,通常数据单位为m,取半径之类得参数要除1000)
2.1 ISS关键点
ISS 关键点通过计算每个点与其近邻点的曲率变化,得到该点的稳定性和自适应尺度,从而提取稳定性和尺度合适的关键点。
Open3d(需要ply格式)
import open3d as o3d
import numpy as np
import time
class o3dtut:def get_bunny_mesh():mesh = o3d.io.read_triangle_mesh("second_radius_cloud.ply")mesh.compute_vertex_normals()return meshdef keypoints_to_spheres(keypoints):spheres = o3d.geometry.TriangleMesh()for keypoint in keypoints.points:sphere = o3d.geometry.TriangleMesh.create_sphere(radius=10)sphere.translate(keypoint)spheres += spherespheres.paint_uniform_color([1.0, 0.0, 0.0])return spheresmesh = o3dtut.get_bunny_mesh()
pcd = o3d.geometry.PointCloud()
pcd.points = mesh.vertices
tic = time.time()
keypoints = o3d.geometry.keypoint.compute_iss_keypoints(pcd,salient_radius=10,non_max_radius=10,gamma_21=0.5,gamma_32=0.5)
toc = 1000 * (time.time() - tic)
print("ISS Computation took {:.0f} [ms]".format(toc))mesh.compute_vertex_normals()
mesh.paint_uniform_color([0.0, 0.0, 1.0])
o3d.visualization.draw_geometries([o3dtut.keypoints_to_spheres(keypoints), mesh],window_name="ISS检测",width=800,height=800)
PCL
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/visualization/pcl_visualizer.h>int main(int argc, char** argv)
{// 读取点云数据pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());if (pcl::io::loadPCDFile<pcl::PointXYZ>("second_radius_cloud.pcd", *cloud) == -1){PCL_ERROR("Couldn't read the input PCD file \n");return (-1);}// ISS关键点检测参数double salient_radius = 10;double non_max_radius = 10;double gamma_21 = 0.5;double gamma_32 = 0.5;double min_neighbors = 5;int threads = 4;// 执行ISS关键点检测pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints(new pcl::PointCloud<pcl::PointXYZ>());pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_detector;iss_detector.setSearchMethod(tree);iss_detector.setSalientRadius(salient_radius);iss_detector.setNonMaxRadius(non_max_radius);iss_detector.setThreshold21(gamma_21);iss_detector.setThreshold32(gamma_32);iss_detector.setMinNeighbors(min_neighbors);iss_detector.setNumberOfThreads(threads);iss_detector.setInputCloud(cloud);iss_detector.compute(*keypoints);// 可视化pcl::visualization::PCLVisualizer viewer("ISS Keypoints");viewer.setBackgroundColor(0, 0, 0);viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");viewer.addPointCloud<pcl::PointXYZ>(keypoints, "keypoints");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "keypoints");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "keypoints");while (!viewer.wasStopped()){viewer.spinOnce(100);}return 0;
}
2.2 Harris关键点
Harris关键点检测通过计算每个点的协方差矩阵,求解特征值和特征向量,来判断该点是否为关键点。如果Harris矩阵的两个特征值都很大,则该点是关键点。具有较好的旋转不变性和尺度不变性。
PCL
#include
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/io.h>
#include <pcl/keypoints/harris_3d.h>//harris特征点估计类头文件声明
#include
#include
#include <pcl/console/parse.h>
using namespace std;
int main()
{
pcl::PointCloudpcl::PointXYZ::Ptr input_cloud (new pcl::PointCloudpcl::PointXYZ);
pcl::io::loadPCDFile (“second_radius_cloud.pcd”, *input_cloud);
pcl::PCDWriter writer;
float r_normal=10;//法向量估计的半径
float r_keypoint=40;//关键点估计的近邻搜索半径
typedef pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> ColorHandlerT3;pcl::PointCloud<pcl::PointXYZI>::Ptr Harris_keypoints (new pcl::PointCloud<pcl::PointXYZI> ());//存放最后的特征点提取结果
pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI,pcl::Normal>* harris_detector = new pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI,pcl::Normal> ;harris_detector->setRadius(r_normal);//设置法向量估计的半径
harris_detector->setRadiusSearch(r_keypoint);//设置关键点估计的近邻搜索半径
harris_detector->setInputCloud (input_cloud);//设置输入点云
harris_detector->compute (*Harris_keypoints);//结果存放在Harris_keypoints
pcl::visualization::PCLVisualizer viewer("clouds");
viewer.setBackgroundColor(255,255,255);
viewer.addPointCloud (Harris_keypoints, ColorHandlerT3 (Harris_keypoints, 0.0, 0.0, 255.0), "Harris_keypoints");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"Harris_keypoints");
viewer.addPointCloud(input_cloud,"input_cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,0,"input_cloud");
viewer.initCameraParameters();
viewer.saveScreenshot("screenshot.png");
viewer.spin ();
}
2.3 NAFR关键点
NARF算法是一种基于法向量的点云关键点提取算法。它通过将点云投影到二维图像上,并计算每个像素周围梯度直方图,来寻找具有唯一性和重复性的关键点。
PCL
注意修改角度范围和角度分辨率
#include <iostream>#include <pcl/range_image/range_image.h> // 深度图头文件#include <pcl/io/pcd_io.h> // PCD文件读取头#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> // NARF关键点计算头#include <pcl/features/narf_descriptor.h>// NARF描述子头#include <pcl/console/parse.h> // 命令行解析头#include <pcl/common/file_io.h> // 用于获取没有拓展名的文件typedef pcl::PointXYZ PointType;float angular_resolution = 0.1875f; // 角度分辨率float support_size = 20.0f; // 关键点计算范围(计算范围球的半径)pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME; // 深度图坐标系bool setUnseenToMaxRange = false; // 不设置深度范围bool rotation_invariant = true; // 是否保持旋转不变性// --------------// -----输出帮助信息-----
// --------------
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);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, "-m") >= 0){setUnseenToMaxRange = true;std::cout << "Setting unseen values in range image to maximum range readings.\n";}if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0)std::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);std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";}if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)std::cout << "Setting support size to "<<support_size<<".\n";if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)std::cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";angular_resolution = pcl::deg2rad (angular_resolution);// 读入PCD点云文件或创造出一些点云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){std::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)+".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;std::cout << "\nNo *.pcd file given => Generating 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.push_back (point);}}point_cloud.width = point_cloud.size (); point_cloud.height = 1;}// 利用点云创造深度图float noise_level = 0.0;float min_range = 0.0f;int border_size = 1;pcl::RangeImage::Ptr range_image_ptr (new pcl::RangeImage);pcl::RangeImage& range_image = *range_image_ptr; range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (60.0f), pcl::deg2rad (45.0f),scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);range_image.integrateFarRanges (far_ranges);if (setUnseenToMaxRange)range_image.setUnseenToMaxRange ();// 显示点云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 ();setViewerPose (viewer, range_image.getTransformationToWorldSystem ());// 显示深度图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; narf_keypoint_detector.setRangeImageBorderExtractor (&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);// 计算narf关键点,并按照序列排序std::cout << "Found "<<keypoint_indices.size ()<<" key points.\n";pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;keypoints.resize (keypoint_indices.size ());for (std::size_t i=0; i<keypoint_indices.size (); ++i)keypoints[i].getVector3fMap () = range_image[keypoint_indices[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");viewer.initCameraParameters();viewer.saveScreenshot("screenshot.png");// 为这些关键点计算NARF描述子std::vector<int> keypoint_indices2;keypoint_indices2.resize (keypoint_indices.size ());for (unsigned int i=0; i<keypoint_indices.size (); ++i) keypoint_indices2[i]=keypoint_indices[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);std::cout << "Extracted "<<narf_descriptors.size ()<<" descriptors for "<<keypoint_indices.size ()<< " keypoints.\n";while (!viewer.wasStopped ()){viewer.spinOnce ();}
}
2.4 SIFT关键点
SIFT3D算法是一种基于高斯差分和尺度空间的点云关键点提取算法。它通过在多个尺度下对点云进行高斯滤波和差分操作,来提取稳定性和尺度不变性的关键点。
PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>namespace pcl
{template<>struct SIFTKeypointFieldSelector<PointXYZ>{inline floatoperator () (const PointXYZ &p) const{return p.z;}};
}int main()
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>("second_radius_cloud.pcd", *cloud_xyz) == -1) // load the file{PCL_ERROR("Couldn't read file");return -1;}std::cout << "points: " << cloud_xyz->points.size() << std::endl;// Parameters for sift computationconst float min_scale = 5.0f; //the standard deviation of the smallest scale in the scale spaceconst int n_octaves = 6;//the number of octaves (i.e. doublings of scale) to computeconst int n_scales_per_octave = 4;//the number of scales to compute within each octaveconst float min_contrast = 1.0f;//the minimum contrast required for detectionpcl::console::TicToc time;time.tic();// Estimate the sift interest points using z values from xyz as the Intensity variantspcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;pcl::PointCloud<pcl::PointWithScale> result;pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());sift.setSearchMethod(tree);sift.setScales(min_scale, n_octaves, n_scales_per_octave);sift.setMinimumContrast(min_contrast);sift.setInputCloud(cloud_xyz);sift.compute(result);std::cout << "Computing the SIFT points takes " << time.toc() / 1000 << "seconds" << std::endl;std::cout << "No of SIFT points in the result are " << result.points.size() << std::endl;// Copying the pointwithscale to pointxyz so as visualize the cloudpcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);copyPointCloud(result, *cloud_temp);std::cout << "SIFT points in the result are " << cloud_temp->points.size() << std::endl;// Visualization of keypoints along with the original cloudpcl::visualization::PCLVisualizer viewer("PCL Viewer");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler(cloud_temp, 0, 255, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler(cloud_xyz, 255, 0, 0);viewer.setBackgroundColor(0.0, 0.0, 0.0);viewer.addPointCloud(cloud_xyz, cloud_color_handler, "cloud");//add point cloudviewer.addPointCloud(cloud_temp, keypoints_color_handler, "keypoints");//add the keypointsviewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");while (!viewer.wasStopped()){viewer.spinOnce();}return 0;}