PCL 法向量估计源码学习

一、思路：

二、源码


#ifndef PCL_FEATURES_IMPL_NORMAL_3D_H_
#define PCL_FEATURES_IMPL_NORMAL_3D_H_#include <pcl/features/normal_3d.h>///
template <typename PointInT, typename PointOutT> void
pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{// Allocate enough space to hold the results// \note This resize is irrelevant for a radiusSearch ().std::vector<int> nn_indices (k_);std::vector<float> nn_dists (k_);output.is_dense = true;// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense//  如果云设置为密集，则不检查每个点的NaN/Inf值，从而节省几个周期if (input_->is_dense){// Iterating over the entire index vector// 迭代索引，计算每个索引对应点的 法向量for (size_t idx = 0; idx < indices_->size (); ++idx){// 判断点云领域内是否有点，并且计算点云的法向量 和 曲率if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||!computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)){output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();output.is_dense = false;continue;}// 设置视角点 可以理解为法相的朝向问题flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);}}else{// Iterating over the entire index vectorfor (size_t idx = 0; idx < indices_->size (); ++idx){if (!isFinite ((*input_)[(*indices_)[idx]]) ||this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||!computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)){output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();output.is_dense = false;continue;}flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);}}
}#define PCL_INSTANTIATE_NormalEstimation(T,NT) template class PCL_EXPORTS pcl::NormalEstimation<T,NT>;#endif    // PCL_FEATURES_IMPL_NORMAL_3D_H_

1、 computePointNormal计算协方差矩阵和曲率

template <typename PointT> inline boolcomputePointNormal (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,Eigen::Vector4f &plane_parameters, float &curvature){// Placeholder for the 3x3 covariance matrix at each surface patchEIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;// 16-bytes aligned placeholder for the XYZ centroid of a surface patchEigen::Vector4f xyz_centroid;// 计算每个点的协方差矩阵和质心if (indices.size () < 3 ||computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, xyz_centroid) == 0){plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());curvature = std::numeric_limits<float>::quiet_NaN ();return false;}// Get the plane normal and surface curvature// 通过协方差矩阵和质心来计算点云的   局部平面 的法相 nx ny  nz  和  局部曲率solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);return true;}inline boolcomputePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,float &nx, float &ny, float &nz, float &curvature){if (indices.size () < 3 ||computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0){nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();return false;}// Get the plane normal and surface curvaturesolvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature);return true;}

 1.1 computeMeanAndCovarianceMatrix 计算协方差矩阵

//
template <typename PointT, typename Scalar> inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,Eigen::Matrix<Scalar, 4, 1> &centroid)
{// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a bufferEigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();size_t point_count;if (cloud.is_dense){point_count = cloud.size ();// For each point in the cloudfor (size_t i = 0; i < point_count; ++i){accu [0] += cloud[i].x * cloud[i].x;accu [1] += cloud[i].x * cloud[i].y;accu [2] += cloud[i].x * cloud[i].z;accu [3] += cloud[i].y * cloud[i].y; // 4accu [4] += cloud[i].y * cloud[i].z; // 5accu [5] += cloud[i].z * cloud[i].z; // 8accu [6] += cloud[i].x;accu [7] += cloud[i].y;accu [8] += cloud[i].z;}}else{point_count = 0;for (size_t i = 0; i < cloud.size (); ++i){if (!isFinite (cloud[i]))continue;accu [0] += cloud[i].x * cloud[i].x;accu [1] += cloud[i].x * cloud[i].y;accu [2] += cloud[i].x * cloud[i].z;accu [3] += cloud[i].y * cloud[i].y;accu [4] += cloud[i].y * cloud[i].z;accu [5] += cloud[i].z * cloud[i].z;accu [6] += cloud[i].x;accu [7] += cloud[i].y;accu [8] += cloud[i].z;++point_count;}}accu /= static_cast<Scalar> (point_count);if (point_count != 0){//centroid.head<3> () = accu.tail<3> ();    -- does not compile with Clang 3.0centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];centroid[3] = 1;covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);}return (static_cast<unsigned int> (point_count));
}

 1.2 solvePlaneParameters计算平面法向量和曲率

//
inline void
pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,const Eigen::Vector4f &point,Eigen::Vector4f &plane_parameters, float &curvature)
{solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);plane_parameters[3] = 0;// Hessian form (D = nc . p_plane (centroid here) + p)plane_parameters[3] = -1 * plane_parameters.dot (point);
}//
inline void
pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,float &nx, float &ny, float &nz, float &curvature)
{// Avoid getting hung on Eigen's optimizers
//  for (int i = 0; i < 9; ++i)
//    if (!pcl_isfinite (covariance_matrix.coeff (i)))
//    {
//      //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
//      nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
//      return;
//    }// Extract the smallest eigenvalue and its eigenvectorEIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);nx = eigen_vector [0];ny = eigen_vector [1];nz = eigen_vector [2];// Compute the curvature surface changefloat eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);if (eig_sum != 0)curvature = fabsf (eigen_value / eig_sum);elsecurvature = 0;
}

2、 flipNormalTowardsViewpoint法向量的方向问题

 /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint* \param point a given point* \param vp_x the X coordinate of the viewpoint* \param vp_y the X coordinate of the viewpoint* \param vp_z the X coordinate of the viewpoint* \param normal the plane normal to be flipped* \ingroup features*/template <typename PointT, typename Scalar> inline voidflipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,  Eigen::Matrix<Scalar, 4, 1>& normal){Eigen::Matrix <Scalar, 4, 1> vp (vp_x - point.x, vp_y - point.y, vp_z - point.z, 0);// Dot product between the (viewpoint - point) and the plane normalfloat cos_theta = vp.dot (normal);// Flip the plane normalif (cos_theta < 0){normal *= -1;normal[3] = 0.0f;// Hessian form (D = nc . p_plane (centroid here) + p)normal[3] = -1 * normal.dot (point.getVector4fMap ());}}/** \brief Flip (in place) the estimated normal of a point towards a given viewpoint* \param point a given point* \param vp_x the X coordinate of the viewpoint* \param vp_y the X coordinate of the viewpoint* \param vp_z the X coordinate of the viewpoint* \param normal the plane normal to be flipped* \ingroup features*/template <typename PointT, typename Scalar> inline voidflipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,Eigen::Matrix<Scalar, 3, 1>& normal){Eigen::Matrix <Scalar, 3, 1> vp (vp_x - point.x, vp_y - point.y, vp_z - point.z);// Flip the plane normalif (vp.dot (normal) < 0)normal *= -1;}/** \brief Flip (in place) the estimated normal of a point towards a given viewpoint* \param point a given point* \param vp_x the X coordinate of the viewpoint* \param vp_y the X coordinate of the viewpoint* \param vp_z the X coordinate of the viewpoint* \param nx the resultant X component of the plane normal* \param ny the resultant Y component of the plane normal* \param nz the resultant Z component of the plane normal* \ingroup features*/template <typename PointT> inline voidflipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,float &nx, float &ny, float &nz){// See if we need to flip any plane normalsvp_x -= point.x;vp_y -= point.y;vp_z -= point.z;// Dot product between the (viewpoint - point) and the plane normalfloat cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);// Flip the plane normalif (cos_theta < 0){nx *= -1;ny *= -1;nz *= -1;}}

2.2 设置参数

 inline voidsetViewPoint (float vpx, float vpy, float vpz){vpx_ = vpx;vpy_ = vpy;vpz_ = vpz;use_sensor_origin_ = false;}/** \brief Get the viewpoint.* \param [out] vpx x-coordinate of the view point* \param [out] vpy y-coordinate of the view point* \param [out] vpz z-coordinate of the view point* \note this method returns the currently used viewpoint for normal flipping.* If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates.* If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0)*/inline voidgetViewPoint (float &vpx, float &vpy, float &vpz){vpx = vpx_;vpy = vpy_;vpz = vpz_;}inline voiduseSensorOriginAsViewPoint (){use_sensor_origin_ = true;if (input_){vpx_ = input_->sensor_origin_.coeff (0);vpy_ = input_->sensor_origin_.coeff (1);vpz_ = input_->sensor_origin_.coeff (2);}else{vpx_ = 0;vpy_ = 0;vpz_ = 0;}}

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

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

相关文章

【stability.ai】SDXL：改进高分辨率图像合成的潜在扩散模型

【stability.ai】SDXL：改进高分辨率图像合成的潜在扩散模型

github&#xff1a;https://github.com/Stability-AI/stablediffusion 试用&#xff1a; https://clipdrop.co/stable-diffusion https://dreamstudio.ai/ 介绍 近年来&#xff0c;深度生成建模在自然语言、音频和视觉媒体等各个数据领域取得了巨大的突破。本文将重点关注视…
阅读更多...
nohup命令解决SpringBoot/java -jar命令启动项目运行一段时间自动停止问题

nohup命令解决SpringBoot/java -jar命令启动项目运行一段时间自动停止问题

问题描述&#xff1a; 在centos7上部署多个springcloud项目。出现了服务莫名其妙会挂掉一两个的问题&#xff0c;重新启动挂掉的服务之后又会出现其他服务挂掉的情况&#xff0c;查看启动日志也并没有发现有异常抛出。令人费解的是所有的服务都是通过nohup java -jar xxx.jar …
阅读更多...
TCP / IP 参考模型

TCP / IP 参考模型

TCP / IP 参考模型 5层参考模型5层参考模型5层模型的数据封装 5层参考模型 5层参考模型 综合 OSI 和 TCP/IP 的优点应用层: 支持各种网络应用 FTP, SMTP, HTTP 传输层: 进程-进程的数据传输 TCP, UDP 网络层: 源主机到目的主机的数据分组路由与转发 IP协议、路由协议等 链路层…
阅读更多...
JS中的扁平化数据转换为树形结构数组

JS中的扁平化数据转换为树形结构数组

递归方法和循环方法&#xff0c;我都写了两种代码&#xff0c;仅供参考。 三种递归方法&#xff0c;两种循环方法&#xff0c;本质上都一样&#xff0c;就是写法不一样&#xff0c;加油&#xff01;&#xff01;&#xff01; // 数据 const arr [{ id: 1, name: 广东, pid:…
阅读更多...
还在手动下载github项目？想要自动化下载github项目？基于python开发项目自动下载模块帮你实现自动下载存储

还在手动下载github项目？想要自动化下载github项目？基于python开发项目自动下载模块帮你实现自动下载存储

GitHub是一个基于Web的代码托管平台和开发者社区。它允许开发者存储、管理和分享他们的代码&#xff0c;并进行版本控制。开发者可以在GitHub上创建仓库来存储项目代码&#xff0c;并使用Git来跟踪和管理代码的变更历史。GitHub提供了一系列协作工具&#xff0c;如问题追踪、Pu…
阅读更多...
Vue实现在线文档预览

Vue实现在线文档预览

目录 背景在线预览Office文档文件在线预览pdf文档预览方案一方案二 Word文档预览Excel文档预览PPT文档预览 纯文本、各种代码文件预览图片文件预览视频文件预览Aliplayer阿里云播放器Xgplayer西瓜播放器Bilibiliplayer仿哔哩哔哩弹幕播放器 音频文件预览在线文档预览项目&#…
阅读更多...
POE级联蓝牙定位系统方案_蓝牙信标，蓝牙网关，级联蓝牙网关

POE级联蓝牙定位系统方案_蓝牙信标，蓝牙网关，级联蓝牙网关

近年来，随着新能源行业的快速发展，在化工厂，核电厂以及电力电厂等企业，对人员定位，人员导航，资产定位，生命体征监测的需求越来越大。传统的蓝牙室内定位方案中蓝牙信标为锂亚电池供电，需定期更换电池且有安全隐患，为更好的服务有蓝牙定位导航，被动人员定位，生命体征…
阅读更多...
MySQL之概述、安装和使用（一）

MySQL之概述、安装和使用（一）

一、概述 关系数据库概述&#xff1a; https://blog.csdn.net/qq_21370419/article/details/128568920 二、数据库的安装 参考我的两篇博客&#xff1a; win10 安装mysql 5.6.36版本_windows 安装mysql5.6_人……杰的博客-CSDN博客 wind 10 安装 mysql 8.0_人……杰的博客…
阅读更多...
50从零开始学Java之万类之王Object是怎么回事？

50从零开始学Java之万类之王Object是怎么回事？

作者&#xff1a;孙玉昌&#xff0c;昵称【一一哥】&#xff0c;另外【壹壹哥】也是我哦 千锋教育高级教研员、CSDN博客专家、万粉博主、阿里云专家博主、掘金优质作者 前言 在前面的文章中&#xff0c;壹哥跟大家说过&#xff0c;Java是面向对象的编程语言&#xff0c;而在面…
阅读更多...
IDEA+SpringBoot+mybatis+SSM+layui+Mysql客户管理系统源码

IDEA+SpringBoot+mybatis+SSM+layui+Mysql客户管理系统源码

IDEASpringBootmybatisSSMlayuiMysql客户管理系统 一、系统介绍1.环境配置 二、系统展示1. 管理员登录2.修改密码3.客户管理4.添加客户5.充值记录管理6.消费记录管理7.客户类型8.添加客户类型 三、部分代码UserMapper.javaLoginController.javaUser.java 四、其他获取源码 一、…
阅读更多...
自动化测试工具——Fitnesse

自动化测试工具——Fitnesse

1 介绍 是一个完全集成的独立wiki和验收测试框架。 1.1、协作工具 由于FitNesse是一个wiki web服务器&#xff0c;它的入门和学习曲线非常低&#xff0c;这使得它成为一个优秀的工具&#xff0c;可以与业务涉众进行协作。 1.2、测试工具 FitNesse中创建的wiki页面作为测试…
阅读更多...
17款奔驰S400加装原厂无钥匙进入系统，提升您的便利性

17款奔驰S400加装原厂无钥匙进入系统，提升您的便利性

奔驰无钥匙进入功能&#xff0c;只要身上装着车钥匙进入车内&#xff0c;车辆就能感应到钥匙的存在&#xff0c;这时只需按下启动键就可启动车辆了 奔驰无钥匙进入功能主要有两大使用体验&#xff0c;首先就是要注意主驾驶位车门的有效检测距离不小于1.5m&#xff0c;其他门钥匙…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023