NARF关键点检测及SAC-IA粗配准

一、生成对应深度图

C++

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/range_image/range_image.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/common_headers.h>
using namespace std;int main(int, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//要配准变化的点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);//目标点云(不变的）if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view1.pcd", *cloud) == -1){PCL_ERROR("加载点云失败\n");}if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view2.pcd", *cloud_target) == -1){PCL_ERROR("加载点云失败\n");}pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;Eigen::Affine3f scene_sensor_pose_src(Eigen::Affine3f::Identity());Eigen::Affine3f scene_sensor_pose_tgt(Eigen::Affine3f::Identity());scene_sensor_pose_src = Eigen::Affine3f(Eigen::Translation3f(cloud->sensor_origin_[0],cloud->sensor_origin_[1], cloud->sensor_origin_[2])) * Eigen::Affine3f(cloud->sensor_orientation_);scene_sensor_pose_tgt = Eigen::Affine3f(Eigen::Translation3f(cloud_target->sensor_origin_[0],cloud_target->sensor_origin_[1], cloud_target->sensor_origin_[2])) * Eigen::Affine3f(cloud_target->sensor_orientation_);//-------------从点云创建深度图像———————————float noise_level = 0.0;float min_range = 0.0f;int border_size = 1;pcl::RangeImage::Ptr range_image_src(new pcl::RangeImage);pcl::RangeImage::Ptr range_image_tgt(new pcl::RangeImage);range_image_src->createFromPointCloud(*cloud, pcl::deg2rad(0.02f), pcl::deg2rad(0.02f),pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose_src, coordinate_frame,noise_level, min_range, border_size);range_image_tgt->createFromPointCloud(*cloud_target, pcl::deg2rad(0.02f), pcl::deg2rad(0.02f),pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose_tgt, coordinate_frame,noise_level, min_range, border_size);boost::shared_ptr <pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer1"));//创建窗口，并设置名字为3D Viewerpcl::visualization::PointCloudColorHandlerCustom < pcl::PointWithRange>range_image_color_handler1(range_image_src, 0, 0, 0);viewer1->setBackgroundColor(1, 1, 1); viewer1->addPointCloud(range_image_src, range_image_color_handler1, "range image");viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");while (!viewer1->wasStopped()){viewer1->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100));}boost::shared_ptr <pcl::visualization::PCLVisualizer> viewer2(new pcl::visualization::PCLVisualizer("3D Viewer2"));//创建窗口，并设置名字为3D Viewerpcl::visualization::PointCloudColorHandlerCustom < pcl::PointWithRange>range_image_color_handler2(range_image_tgt, 0, 0, 0);viewer2->setBackgroundColor(1, 1, 1);viewer2->addPointCloud(range_image_tgt, range_image_color_handler2, "range image");viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");while (!viewer2->wasStopped()){viewer2->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100));}boost::shared_ptr <pcl::visualization::PCLVisualizer> viewer3(new pcl::visualization::PCLVisualizer("3D Viewer3"));//创建窗口，并设置名字为3D Viewerpcl::visualization::PointCloudColorHandlerCustom < pcl::PointWithRange>range_image_color_handler3(range_image_src, 255, 0, 0);pcl::visualization::PointCloudColorHandlerCustom < pcl::PointWithRange>range_image_color_handler4(range_image_tgt, 0, 0, 0);viewer3->setBackgroundColor(1, 1, 1);viewer3->addPointCloud(range_image_src, range_image_color_handler3, "range image1");viewer3->addPointCloud(range_image_tgt, range_image_color_handler4, "range image2");viewer3->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image1");viewer3->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image2");while (!viewer3->wasStopped()){viewer3->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100));}return 0;
}

关键代码解析：

pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;Eigen::Affine3f scene_sensor_pose_src(Eigen::Affine3f::Identity());Eigen::Affine3f scene_sensor_pose_tgt(Eigen::Affine3f::Identity());scene_sensor_pose_src = Eigen::Affine3f(Eigen::Translation3f(cloud->sensor_origin_[0],cloud->sensor_origin_[1], cloud->sensor_origin_[2])) * Eigen::Affine3f(cloud->sensor_orientation_);scene_sensor_pose_tgt = Eigen::Affine3f(Eigen::Translation3f(cloud_target->sensor_origin_[0],cloud_target->sensor_origin_[1], cloud_target->sensor_origin_[2])) * Eigen::Affine3f(cloud_target->sensor_orientation_);//-------------从点云创建深度图像———————————float noise_level = 0.0;float min_range = 0.0f;int border_size = 1;pcl::RangeImage::Ptr range_image_src(new pcl::RangeImage);pcl::RangeImage::Ptr range_image_tgt(new pcl::RangeImage);range_image_src->createFromPointCloud(*cloud, pcl::deg2rad(0.02f), pcl::deg2rad(0.02f),pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose_src, coordinate_frame,noise_level, min_range, border_size);range_image_tgt->createFromPointCloud(*cloud_target, pcl::deg2rad(0.02f), pcl::deg2rad(0.02f),pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose_tgt, coordinate_frame,noise_level, min_range, border_size);

  1. pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;：这里定义了深度图像的坐标系，常用的坐标系包括相机坐标系 (CAMERA_FRAME) 和激光雷达坐标系 (LASER_FRAME)。

  2. Eigen::Affine3f scene_sensor_pose_src(Eigen::Affine3f::Identity()); 和 Eigen::Affine3f scene_sensor_pose_tgt(Eigen::Affine3f::Identity());：创建了两个 Affine3f 类型的对象，用于表示点云的姿态（位置和方向）。

  3. 通过以下代码计算了源点云和目标点云的姿态（位置和方向）：

    scene_sensor_pose_src = Eigen::Affine3f(Eigen::Translation3f(cloud->sensor_origin_[0],cloud->sensor_origin_[1], cloud->sensor_origin_[2])) * Eigen::Affine3f(cloud->sensor_orientation_);
scene_sensor_pose_tgt = Eigen::Affine3f(Eigen::Translation3f(cloud_target->sensor_origin_[0],cloud_target->sensor_origin_[1], cloud_target->sensor_origin_[2])) * Eigen::Affine3f(cloud_target->sensor_orientation_);

  4. 设置了创建深度图像所需的参数：

    • float noise_level = 0.0;：噪声水平，表示深度图像中的噪声级别。
    • float min_range = 0.0f;：最小测量范围，表示深度图像中的最小测量距离。
    • int border_size = 1;：边界大小，表示深度图像周围的边界大小。

  5. 创建了两个深度图像对象的指针：pcl::RangeImage::Ptr range_image_src(new pcl::RangeImage); 和 pcl::RangeImage::Ptr range_image_tgt(new pcl::RangeImage);

  6. 通过以下代码，使用点云数据创建深度图像：

    range_image_src->createFromPointCloud(*cloud, pcl::deg2rad(0.02f), pcl::deg2rad(0.02f),pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose_src, coordinate_frame,noise_level, min_range, border_size);
range_image_tgt->createFromPointCloud(*cloud_target, pcl::deg2rad(0.02f), pcl::deg2rad(0.02f),pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose_tgt, coordinate_frame,noise_level, min_range, border_size);

在这里，参数的设置会影响深度图像的质量和内容。例如，noise_level、min_range 和 border_size 参数会影响深度图像的噪声水平、最小测量范围以及边界大小。而角度参数 pcl::deg2rad() 用于将角度转换为弧度。

结果：

输入点云的深度图 

输出点云的深度图  

 

 输入点云和输出点云放在一起的深度图 ，为了区别，我把输入点云深度图颜色改成红色

 

二、 NARF关键点检测

C++

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/common_headers.h>
#include <pcl/range_image/range_image.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>
using namespace std;int main(int, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//要配准变化的点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);//目标点云(不变的）if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view1.pcd", *cloud) == -1){PCL_ERROR("加载点云失败\n");}if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view2.pcd", *cloud_target) == -1){PCL_ERROR("加载点云失败\n");}cout << "读取原点云个数: " << cloud->points.size() << endl;cout << "读取目标点云个数: " << cloud_target->points.size() << endl;pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;Eigen::Affine3f scene_sensor_pose_src(Eigen::Affine3f::Identity());Eigen::Affine3f scene_sensor_pose_tgt(Eigen::Affine3f::Identity());scene_sensor_pose_src = Eigen::Affine3f(Eigen::Translation3f(cloud->sensor_origin_[0],cloud->sensor_origin_[1], cloud->sensor_origin_[2])) * Eigen::Affine3f(cloud->sensor_orientation_);scene_sensor_pose_tgt = Eigen::Affine3f(Eigen::Translation3f(cloud_target->sensor_origin_[0],cloud_target->sensor_origin_[1], cloud_target->sensor_origin_[2])) * Eigen::Affine3f(cloud_target->sensor_orientation_);//-------------从点云创建深度图像———————————float noise_level = 0.0;float min_range = 0.0f;int border_size = 1;pcl::RangeImage::Ptr range_image_src(new pcl::RangeImage);pcl::RangeImage::Ptr range_image_tgt(new pcl::RangeImage);range_image_src->createFromPointCloud(*cloud, pcl::deg2rad(0.02f), pcl::deg2rad(0.02f),pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose_src, coordinate_frame,noise_level, min_range, border_size);range_image_tgt->createFromPointCloud(*cloud_target, pcl::deg2rad(0.02f), pcl::deg2rad(0.02f),pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose_tgt, coordinate_frame,noise_level, min_range, border_size);//range_image->setUnseenToMaxRange();//设置不能观察到的点都为远距离点//-----------提取NARF关键点-------------pcl::RangeImage& range_image1 = *range_image_src;pcl::RangeImageBorderExtractor range_image_border_extractor1;pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZ>());pcl::NarfKeypoint narf_keypoint_detector1(&range_image_border_extractor1);narf_keypoint_detector1.setRangeImage(&range_image1);//指定深度图narf_keypoint_detector1.getParameters().support_size = 0.2f;//指定搜索空间球体的半径，指定计算感兴趣值时所使用的领域范围narf_keypoint_detector1.getParameters().add_points_on_straight_edges = true;//指点是否添加垂直边缘上的点narf_keypoint_detector1.getParameters().distance_for_additional_points = 0.5;pcl::PointCloud<int>keypoint_indices1;narf_keypoint_detector1.compute(keypoint_indices1);//计算索引keypoints1->points.resize(keypoint_indices1.size());keypoints1->height = keypoint_indices1.height;keypoints1->width = keypoint_indices1.width;for (std::size_t i = 0; i < keypoint_indices1.points.size(); ++i){keypoints1->points[i].getVector3fMap() = range_image_src->points[keypoint_indices1.points[i]].getVector3fMap();}cout << "NARF points 的原点云提取结果为 " << keypoints1->points.size() << endl;pcl::RangeImage& range_image2 = *range_image_tgt;pcl::RangeImageBorderExtractor range_image_border_extractor2;pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints2(new pcl::PointCloud<pcl::PointXYZ>());pcl::NarfKeypoint narf_keypoint_detector2(&range_image_border_extractor2);narf_keypoint_detector2.setRangeImage(&range_image2);//指定深度图narf_keypoint_detector2.getParameters().support_size = 0.2f;//指定搜索空间球体的半径，指定计算感兴趣值时所使用的领域范围narf_keypoint_detector2.getParameters().add_points_on_straight_edges = true;//指点是否添加垂直边缘上的点narf_keypoint_detector2.getParameters().distance_for_additional_points = 0.5;pcl::PointCloud<int>keypoint_indices2;narf_keypoint_detector2.compute(keypoint_indices2);//计算索引keypoints2->points.resize(keypoint_indices2.size());keypoints2->height = keypoint_indices2.height;keypoints2->width = keypoint_indices2.width;for (std::size_t i = 0; i < keypoint_indices2.points.size(); ++i){keypoints2->points[i].getVector3fMap() = range_image_tgt->points[keypoint_indices2.points[i]].getVector3fMap();}cout << "NARF points 的目标点云提取结果为 " << keypoints2->points.size() << endl;//关键点显示boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("V1"));viewer1->setBackgroundColor(0, 0, 0);viewer1->setWindowName("NARF Key point extraction");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud, 0.0, 255, 0.0);viewer1->addPointCloud<pcl::PointXYZ>(keypoints1, single_color1, "key cloud");//特征点viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key cloud");while (!viewer1->wasStopped()){viewer1->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100));}boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer2(new pcl::visualization::PCLVisualizer("V2"));viewer2->setBackgroundColor(0, 0, 0);viewer2->setWindowName("NARF Key point extraction");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud, 0.0, 255, 0.0);viewer2->addPointCloud<pcl::PointXYZ>(keypoints2, single_color2, "key cloud");//特征点viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key cloud");while (!viewer2->wasStopped()){viewer2->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100));}boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer3(new pcl::visualization::PCLVisualizer("V3"));viewer3->setBackgroundColor(0, 0, 0);viewer3->setWindowName("NARF Key point extraction");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(cloud, 0.0, 255, 0.0);viewer3->addPointCloud<pcl::PointXYZ>(cloud, single_color3, "sample cloud");viewer3->addPointCloud<pcl::PointXYZ>(keypoints1, "key cloud");//特征点viewer3->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key cloud");viewer3->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "key cloud");while (!viewer3->wasStopped()){viewer3->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100));}boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer4(new pcl::visualization::PCLVisualizer("V4"));viewer4->setBackgroundColor(0, 0, 0);viewer4->setWindowName("NARF Key point extraction");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color4(cloud, 0.0, 255, 0.0);viewer4->addPointCloud<pcl::PointXYZ>(cloud_target, single_color4, "sample cloud");viewer4->addPointCloud<pcl::PointXYZ>(keypoints2, "key cloud");//特征点viewer4->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key cloud");viewer4->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "key cloud");while (!viewer4->wasStopped()){viewer4->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100));}return 0;
}

关键代码解析： 

pcl::RangeImage& range_image1 = *range_image_src;pcl::RangeImageBorderExtractor range_image_border_extractor1;pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZ>());pcl::NarfKeypoint narf_keypoint_detector1(&range_image_border_extractor1);narf_keypoint_detector1.setRangeImage(&range_image1);//指定深度图narf_keypoint_detector1.getParameters().support_size = 0.2f;//指定搜索空间球体的半径，指定计算感兴趣值时所使用的领域范围narf_keypoint_detector1.getParameters().add_points_on_straight_edges = true;//指点是否添加垂直边缘上的点narf_keypoint_detector1.getParameters().distance_for_additional_points = 0.5;pcl::PointCloud<int>keypoint_indices1;narf_keypoint_detector1.compute(keypoint_indices1);//计算索引keypoints1->points.resize(keypoint_indices1.size());keypoints1->height = keypoint_indices1.height;keypoints1->width = keypoint_indices1.width;for (std::size_t i = 0; i < keypoint_indices1.points.size(); ++i){keypoints1->points[i].getVector3fMap() = range_image_src->points[keypoint_indices1.points[i]].getVector3fMap();}

  1. pcl::RangeImage& range_image1 = *range_image_src;：将源深度图像的引用保存到 range_image1 中，以便后续使用。

  2. pcl::RangeImageBorderExtractor range_image_border_extractor1;：创建深度图像边界提取器的实例，用于边界处理。

  3. pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZ>());：创建一个点云对象 keypoints1 用于存储提取的关键点。

  4. pcl::NarfKeypoint narf_keypoint_detector1(&range_image_border_extractor1);：创建 NARF 关键点检测器的实例，并将边界提取器传递给它。

  5. narf_keypoint_detector1.setRangeImage(&range_image1);：指定 NARF 关键点检测器要使用的深度图像。

  6. narf_keypoint_detector1.getParameters().support_size = 0.2f;：设置搜索空间球体的半径，该参数影响关键点的计算感兴趣值时所使用的领域范围。较小的值可能导致检测到更细小的关键点。

  7. narf_keypoint_detector1.getParameters().add_points_on_straight_edges = true;：指定是否在垂直边缘上添加点。设置为 true 可以增加关键点的数量，但可能也增加噪声。

  8. narf_keypoint_detector1.getParameters().distance_for_additional_points = 0.5;：设置额外点之间的距离。该参数用于在一些特殊情况下添加额外的关键点。

  9. pcl::PointCloud<int> keypoint_indices1;：创建一个整数类型的点云对象，用于存储关键点的索引。

  10. narf_keypoint_detector1.compute(keypoint_indices1);：计算关键点的索引。

  11. keypoints1->points.resize(keypoint_indices1.size());：调整关键点点云的大小。

  12. 使用循环将关键点的坐标从深度图像的索引中提取并存储到 keypoints1 中。

结果：

输入点云的关键点

输出点云的关键点 

 

输入点云的关键点与输入点云一起展示

 

输出点云的关键点与输出点云一起展示 

三、NARF关键点检测及SAC-IA粗配准 

C++

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/fpfh_omp.h>  
#include <pcl/common/common_headers.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/range_image/range_image.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>using namespace std;
void extract_keypoint(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& keypoint)
{pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());scene_sensor_pose = Eigen::Affine3f(Eigen::Translation3f(cloud->sensor_origin_[0],cloud->sensor_origin_[1], cloud->sensor_origin_[2])) * Eigen::Affine3f(cloud->sensor_orientation_);//-------------从点云创建深度图像———————————float noise_level = 0.0;float min_range = 0.0f;int border_size = 1;pcl::RangeImage::Ptr range_image(new pcl::RangeImage);range_image->createFromPointCloud(*cloud, pcl::deg2rad(0.02f), pcl::deg2rad(0.02f),pcl::deg2rad(360.0f), pcl::deg2rad(180.0f), scene_sensor_pose, coordinate_frame,noise_level, min_range, border_size);//range_image->setUnseenToMaxRange();//设置不能观察到的点都为远距离点//-----------提取NARF关键点-------------pcl::RangeImage& range_image0 = *range_image;pcl::RangeImageBorderExtractor range_image_border_extractor;pcl::NarfKeypoint narf_keypoint_detector(&range_image_border_extractor);narf_keypoint_detector.setRangeImage(&range_image0);//指定深度图narf_keypoint_detector.getParameters().support_size = 0.2f;//指定搜索空间球体的半径，指定计算感兴趣值时所使用的领域范围narf_keypoint_detector.getParameters().add_points_on_straight_edges = true;//指点是否添加垂直边缘上的点narf_keypoint_detector.getParameters().distance_for_additional_points = 0.5;pcl::PointCloud<int>keypoint_indices;narf_keypoint_detector.compute(keypoint_indices);//计算索引keypoint->points.resize(keypoint_indices.size());keypoint->height = keypoint_indices.height;keypoint->width = keypoint_indices.width;for (std::size_t i = 0; i < keypoint_indices.points.size(); ++i){keypoint->points[i].getVector3fMap() = range_image->points[keypoint_indices.points[i]].getVector3fMap();}}
pcl::PointCloud<pcl::FPFHSignature33>::Ptr compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZ>::Ptr& keypoint)
{pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;n.setInputCloud(keypoint);n.setSearchMethod(tree);n.setKSearch(10);n.compute(*normals);pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> f;f.setNumberOfThreads(8);f.setInputCloud(keypoint);f.setInputNormals(normals);f.setSearchMethod(tree);f.setRadiusSearch(50);f.compute(*fpfh);return fpfh;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr sac_align(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr s_k, pcl::PointCloud<pcl::PointXYZ>::Ptr t_k, pcl::PointCloud<pcl::FPFHSignature33>::Ptr sk_fpfh, pcl::PointCloud<pcl::FPFHSignature33>::Ptr tk_fpfh)
{pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;scia.setInputSource(s_k);scia.setInputTarget(t_k);scia.setSourceFeatures(sk_fpfh);scia.setTargetFeatures(tk_fpfh);scia.setMinSampleDistance(7);///参数：设置采样点之间的最小距离，满足的被当做采样点scia.setNumberOfSamples(250);设置每次迭代设置采样点的个数(这个参数多可以增加配准精度)scia.setCorrespondenceRandomness(4);//设置选择随机特征对应点时要使用的邻域点个数。值越大，特征匹配的随机性就越大/*scia.setEuclideanFitnessEpsilon(0.001);scia.setTransformationEpsilon(1e-10);scia.setRANSACIterations(30);*/pcl::PointCloud<pcl::PointXYZ>::Ptr sac_result(new pcl::PointCloud<pcl::PointXYZ>);scia.align(*sac_result);pcl::transformPointCloud(*cloud, *sac_result, scia.getFinalTransformation());return sac_result;
}
int main(int, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//要配准变化的点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);//目标点云(不变的）if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view1.pcd", *cloud) == -1){PCL_ERROR("加载点云失败\n");}if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view2.pcd", *cloud_target) == -1){PCL_ERROR("加载点云失败\n");}boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer1(new pcl::visualization::PCLVisualizer("v1"));viewer1->setBackgroundColor(0, 0, 0);  //设置背景颜色为黑色// 对目标点云着色可视化 (red).pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color1(cloud_target, 255, 0, 0);viewer1->addPointCloud<pcl::PointXYZ>(cloud_target, target_color1, "target cloud");viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");// 对源点云着色可视化 (green).pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color1(cloud, 0, 255, 0);viewer1->addPointCloud<pcl::PointXYZ>(cloud, input_color1, "input cloud");viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input cloud");//while (!viewer1->wasStopped()){viewer1->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100));}////粗配准pcl::PointCloud<pcl::PointXYZ>::Ptr s_k(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr t_k(new pcl::PointCloud<pcl::PointXYZ>);extract_keypoint(cloud, s_k);extract_keypoint(cloud_target, t_k);std::cout << "关键点数量" << s_k->size() << std::endl;std::cout << "关键点数量" << t_k->size() << std::endl;pcl::PointCloud<pcl::FPFHSignature33>::Ptr sk_fpfh = compute_fpfh_feature(s_k);pcl::PointCloud<pcl::FPFHSignature33>::Ptr tk_fpfh = compute_fpfh_feature(t_k);pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);result = sac_align(cloud, s_k, t_k, sk_fpfh, tk_fpfh);boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer2(new pcl::visualization::PCLVisualizer("显示点云"));viewer2->setBackgroundColor(0, 0, 0);  //设置背景颜色为黑色// 对目标点云着色可视化 (red).pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color2(cloud_target, 255, 0, 0);viewer2->addPointCloud<pcl::PointXYZ>(cloud_target, target_color2, "target cloud");viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");// 对源点云着色可视化 (green).pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color2(result, 0, 255, 0);viewer2->addPointCloud<pcl::PointXYZ>(result, input_color2, "input cloud");viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input cloud");//while (!viewer2->wasStopped()){viewer2->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100));}return 0;
}

关键代码解析： 

我之前在iss关键点检测以及SAC-IA粗配准-CSDN博客

已经解释了大部分函数，这篇文章就不赘述了

range_image->setUnseenToMaxRange();

 这一行是上述代码被注释掉的部分，稍微解释一下

setUnseenToMaxRange() 被调用后，范围图像中未被看到的点（通常由传感器视野限制或遮挡造成）的范围值会被设置为最大可能的范围值

scia.setEuclideanFitnessEpsilon(0.001);
scia.setTransformationEpsilon(1e-10);
scia.setRANSACIterations(30);

这三行也是上述代码被注释掉的部分，稍微解释一下

  1. scia.setEuclideanFitnessEpsilon(0.001);：

    • 这一行代码设置了模型配准中的欧氏距离收敛标准（Euclidean Fitness Epsilon）。
    • 欧氏距离是指在配准的过程中，通过优化变换矩阵，使得目标点云与源点云的每个点之间的欧氏距离小于该阈值。
    • 参数值 0.001 表示当欧氏距离小于或等于 0.001 时，认为配准已经收敛。

  2. scia.setTransformationEpsilon(1e-10);：

    • 这一行代码设置了模型配准中的变换矩阵收敛标准（Transformation Epsilon）。
    • 变换矩阵收敛标准是指通过优化变换矩阵的过程中，变换矩阵的变化小于该阈值时，认为配准已经收敛。
    • 参数值 1e-10 是一个非常小的数，表示当变换矩阵的变化小于或等于 1e-10 时，认为配准已经收敛。

  3. scia.setRANSACIterations(30);：

    • 这一行代码设置了模型配准中的 RANSAC 迭代次数。
    • RANSAC（Random Sample Consensus）是一种迭代优化的方法，通过随机采样来估计变换矩阵。
    • 参数值 30 表示进行 30 次 RANSAC 迭代来估计最优的变换矩阵。

影响：

  • 调整 setEuclideanFitnessEpsilon 的值会影响收敛的条件，更小的值可能导致更严格的收敛，但也可能增加算法运行时间。
  • 调整 setTransformationEpsilon 的值会影响变换矩阵的收敛条件，较小的值表示更精确的匹配，但也可能增加计算开销。
  • 调整 setRANSACIterations 的值会影响 RANSAC 迭代的次数，更多的迭代次数可能带来更精确的变换矩阵，但也增加了计算时间。

结果：

输入点云与输出点云

配准后的输入点云与输出点云，实际效果并不好，运行也慢，需要好几分钟

 

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/news/683435.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

动态内存管理：new和delete的底层探索

动态内存管理：new和delete的底层探索

之前我们在C语言上是学过malloc和calloc还要realloc等函数来在堆上获取相应的内存&#xff0c;但是这些函数是存在缺陷的&#xff0c;今天引入对new和delete的学习&#xff0c;来了解new和delete的底层实现。 首先就是在C中我们为什么要对内存进行区域的分块&#xff1f; 答案…
阅读更多...
SpaCy的使用例子总结

SpaCy的使用例子总结

当使用Spacy进行自然语言处理时&#xff0c;常见的用例包括文本分词、命名实体识别、词性标注、句法分析等。下面是一些常见的使用例子及相应的代码&#xff1a; 文本分词&#xff08;Tokenization&#xff09;&#xff1a; 将文本划分成单词或标点符号等基本单元。 import …
阅读更多...
数据分析 — Pandas 分组聚合

数据分析 — Pandas 分组聚合

目录 一、函数应用和映射1、apply2、map 二、汇总和描述统计1、计算平均值2、计算中位数3、计算总和4、找到最小值5、找到最大值6、计算标准差7、计算方差8、计算非空值的数量9、生成摘要统计信息10、计算唯一值的频率 三、str 属性1、str.len()2、str.lower() 和 str.upper()3…
阅读更多...
【数据结构】单调栈

【数据结构】单调栈

参考&#xff1a;算法学习笔记(67): 单调栈 单调栈用来查找比当前元素大的第一个元素&#xff08;可以修改成比当前元素小的第一个元素&#xff09; 要注意下方代码中栈中存的是下标不是值 stack<int> stk; // 存的是还没有确定下一个比自身大的元素的元素下标 for (i…
阅读更多...
ChatGPT高效提问—prompt实践（漏洞风险分析-重构建议-识别内存泄漏）

ChatGPT高效提问—prompt实践（漏洞风险分析-重构建议-识别内存泄漏）

ChatGPT高效提问—prompt实践&#xff08;漏洞风险分析-重构建议-识别内存泄漏&#xff09; 1.1 漏洞和风险分析 ChatGPT还可以帮助开发人员预测代码的潜在风险&#xff0c;识别其中的安全漏洞&#xff0c;而不必先运行它&#xff0c;这可以让开发人员及早发现错误&#xff0…
阅读更多...
【vscode】在vscode中如何导入自定义包

【vscode】在vscode中如何导入自定义包

只需要额外添加这两条语句即可&#xff1a; import os,sys sys.path.append("../..") 需要注意的是&#xff0c;ipynb 文件打开的工作目录是文件本身的路径&#xff0c;而 py 文件打开的工作路径是 vscode 打开的路径。 相比较而言 pycharm 中创建好项目之后并不…
阅读更多...
FT2232调试记录（2）

FT2232调试记录（2）

FT2232调试记录 &#xff08;1&#xff09;获取当前连接的FTDI设备通道个数:&#xff08;2&#xff09;获取当前连接的设备通道的信息:&#xff08;3&#xff09;配置SPI的通道:&#xff08;4&#xff09;如何设置GPIO:&#xff08;5&#xff09;DEMO测试&#xff1a; FT2232调…
阅读更多...
代码随想录刷题第32天

代码随想录刷题第32天

今天继续贪心算法的学习。第一题是买卖股票的最佳时机https://leetcode.cn/problems/best-time-to-buy-and-sell-stock-ii/description/&#xff0c;题目很唬人&#xff0c;但事实上就是遍历一遍数组&#xff0c;求出所有利润为正的情况加和就行&#xff0c;代码很简单。 clas…
阅读更多...
【阅读笔记】空域保边降噪《Side Window Filtering》

【阅读笔记】空域保边降噪《Side Window Filtering》

1、保边滤波背景 保边滤波器的代表包括双边滤波、引导滤波&#xff0c;但是这类滤波器有一个问题&#xff0c;它们均将待处理的像素点放在了方形滤波窗口的中心。但如果待处理的像素位于图像纹理或者边缘&#xff0c;方形滤波核卷积的处理结果会导致这个边缘变模糊。 基于这个…
阅读更多...
揭秘 2024 春晚刘谦魔术——代码还原

揭秘 2024 春晚刘谦魔术——代码还原

其他系列文章导航 Java基础合集数据结构与算法合集 设计模式合集 多线程合集 分布式合集 ES合集 文章目录 其他系列文章导航 文章目录 前言 一、魔术大概流程 二、代码实现各个步骤 2.1 partition&#xff08;对半撕牌&#xff09; 2.2 bottom&#xff08;将 n 张牌置底…
阅读更多...
仿生学是什么，举出一些通俗的应用案例和应用算法？比如蝙蝠和雷达，鸟和飞机，鱼和船属于仿生学吗？灰狼算法、蚁群算法、麻雀算法属于仿生学吗？除了这些案例还有哪些？

仿生学是什么，举出一些通俗的应用案例和应用算法？比如蝙蝠和雷达，鸟和飞机，鱼和船属于仿生学吗？灰狼算法、蚁群算法、麻雀算法属于仿生学吗？除了这些案例还有哪些？

问题描述&#xff1a;仿生学是什么&#xff0c;举出一些通俗的应用案例和应用算法&#xff1f;比如蝙蝠和雷达&#xff0c;鸟和飞机&#xff0c;鱼和船属于仿生学吗&#xff1f;灰狼算法、蚁群算法、麻雀算法属于仿生学吗&#xff1f;除了这些案例还有哪些&#xff1f; 问题解…
阅读更多...
基于微信小程序的智能社区服务小程序，附源码

基于微信小程序的智能社区服务小程序，附源码

博主介绍&#xff1a;✌程序员徐师兄、7年大厂程序员经历。全网粉丝12w、csdn博客专家、掘金/华为云/阿里云/InfoQ等平台优质作者、专注于Java技术领域和毕业项目实战✌ &#x1f345;文末获取源码联系&#x1f345; &#x1f447;&#x1f3fb; 精彩专栏推荐订阅&#x1f447;…
阅读更多...
谈谈Lombok的坑

谈谈Lombok的坑

Lombok 是一个 Java 库&#xff0c;通过注解的方式在编译时自动为类生成 getter、setter、equals、hashCode 等方法&#xff0c;以简化代码和提高开发效率。本文主要谈谈代码简化背后的代价。 引入Lombok之前是怎么做的 IDE中添加getter/setter, toString等代码&#xff1a; …
阅读更多...
单链表的介绍

单链表的介绍

一.单链表的概念及结构 概念&#xff1a;链表是⼀种物理存储结构上⾮连续、⾮顺序的存储结构&#xff0c;数据元素的逻辑顺序是通过链表 中的指针链接次序实现的 。 结构&#xff1a;根据个人理解&#xff0c;链表的结构就像火车厢一样&#xff0c;一节一节连在一起的&#x…
阅读更多...
蓝桥杯（Web大学组）2022省赛真题：冬奥大抽奖

蓝桥杯（Web大学组）2022省赛真题：冬奥大抽奖

思路&#xff1a; 使用模板字符串&#xff0c;借助time的值选择添加或移除样式的盒子&#xff0c;由于盒子的类名最多为li9&#xff0c;所以要将time的值取余&#xff0c;且判断余数为0时&#xff0c;就取1&#xff0c;否则会获取空值报错 .ul .li${time%9!0?time%9:1} 代码…
阅读更多...
Dataframe型数据分析技巧汇总

Dataframe型数据分析技巧汇总

Kaggle 如何针对少量数据集比赛的打法。 数据降维的几种方法 HF.075 | 时间序列趋势性分析方法汇总 机器学习必须了解的7种交叉验证方法&#xff08;附代码&#xff09; 这个图&#xff01;Python也能一键绘制了&#xff0c;而且样式更多.. 散点图&#xff0c;把散点图画出花来…
阅读更多...
Selenium折线图自动化测试

Selenium折线图自动化测试

目录 获取折线图echarts实例 获取折线图实例锚点的坐标 通过echarts实例的getOption()方法获取坐标数据 将折线图坐标点转换为像素坐标值 整合折线图坐标数据 根据折线图坐标计算出锚点相对于浏览器中的坐标 计算canvas画布原点的坐标 计算折线图相对于浏览器的坐标 使用…
阅读更多...
实现安全性

实现安全性

实现安全性 问题陈述 Chris希望阅读位于服务器上的电子邮件消息。他将自己的登录信息发送到服务器已进行验证。因此，Chris决定用基于表单的验证来验证他的登录信息。但是，他首先决定只用基于表单的验证测试登录页面 。 解决方案 要解决上述问题，Chris需要执行以下任务： 用…
阅读更多...
2.14学习总结

2.14学习总结

1.区间嵌套 https://www.acwing.com/problem/content/description/5462/ 2.卡片 https://www.lanqiao.cn/problems/1443/learning/?page1&first_category_id1&second_category_id3&name%E5%8D%A1%E7%89%87 3.逆序对https://www.luogu.com.cn/problem/P1908 4.合唱…
阅读更多...
不等式的证明之一

不等式的证明之一

不等式的证明 证明下述不等式之一证明 证明下述不等式之一 设 a , b , c a,b,c a,b,c 是正实数&#xff0c;请证明下述不等式&#xff1a; 1 < a a 2 b 2 b b 2 c 2 c c 2 a 2 ≤ 3 2 1<\frac{a}{\sqrt{a^2 b^2}} \frac{b}{\sqrt{b^2 c^2}} \frac{c}{\sqrt{c…
阅读更多...
最新文章

Copyright @ 2022~2023