pcl最小二乘求点云平面度_pcl求点到平面的距离-程序员宅基地

技术标签：平面

平面方程

$z=ax+by+c$

N个点,坐标为

$(x_i,y_i,z_i)$

点到平面的距离

$d_i=\frac{ax_i+by_i+c-z_i}{\sqrt{a^2+b^2+1}}$

平面度

$F=d_{max}-d_{min}$

目标函数

$f(a,b,c) = \sum (ax_i+y_i+x-z_i)^2$

偏导为0

$\frac{\partial f }{\partial a}=\frac{\partial f }{\partial b}=\frac{\partial f }{\partial c}=0$

得到矩阵

$\begin{bmatrix} \sum x_i^2 & \sum x_iy_i & \sum x_i \\ \sum x_iy_i & \sum y_i^2&\sum y_i \\ \sum x_i& \sum y_i &N \end{bmatrix}\begin{bmatrix} a\\ b\\ c \end{bmatrix}=\begin{bmatrix} \sum x_iz_i \\ \sum z_iy_i \\ \sum z_i \end{bmatrix}$

令

$A=\begin{bmatrix} \sum x_i^2 & \sum x_iy_i & \sum x_i \\ \sum x_iy_i & \sum y_i^2&\sum y_i \\ \sum x_i& \sum y_i &N \end{bmatrix}$ , $X=\begin{bmatrix} a\\ b\\ c \end{bmatrix}$ , $b=\begin{bmatrix} \sum x_iz_i \\ \sum z_iy_i \\ \sum z_i \end{bmatrix}$

解下来就是解线性方程的问题了,得到的解X就是平面系数.

接下来求得所有点到该平面的距离(这里的距离要带方向,区分平面上下的点),将距平面上方一个最远的点和平面上方最远的点作差即可,也就是最大距离减去最小距离.

该方法只适合点云数量在千万级以下的,在千万级以上的需要另做处理.

附上部分代码.

float LS(pcl::PointCloud<PointT>::Ptr cloud) {
    pcl::console::TicToc time; time.tic();

    auto xyz_id = cloud->getMatrixXfMap(3, 4, 0);

    Eigen::Vector3f xyz_id_mean = xyz_id.rowwise().mean();                                 
    Eigen::MatrixXf xyz_id_centriod = xyz_id.colwise() - xyz_id_mean;

    Eigen::Vector3f xyz_all_Sum = xyz_id_centriod.rowwise().sum();                          //计算最小二乘法里矩阵A以及b的各元素值
    Eigen::Vector3f xyz_all_Pow2_Sum = xyz_id_centriod.array().square().rowwise().sum();
    auto xy_all_sum = xyz_id_centriod.row(0).dot(xyz_id_centriod.row(1).transpose());
    auto xz_all_sum = xyz_id_centriod.row(0).dot(xyz_id_centriod.row(2).transpose());
    auto yz_all_sum = xyz_id_centriod.row(1).dot(xyz_id_centriod.row(2).transpose());

    Eigen::Matrix3f A;
    A << xyz_all_Pow2_Sum(0), xy_all_sum, xyz_all_Sum(0),
        xy_all_sum, xyz_all_Pow2_Sum(1), xyz_all_Sum(1),
        xyz_all_Sum(0), xyz_all_Sum(1), cloud->size();
    Eigen::Vector3f B;
    B << xz_all_sum, yz_all_sum, xyz_all_Sum(2);
    Eigen::Vector3f X = A.lu().solve(B);                                                    

    Eigen::Vector4f plane;
    plane << X(0), X(1), -1, X(2);                                                          
    float dataMin = INT_MAX, dataMax = INT_MIN;

    for (int j = 0; j < cloud->size(); j++) {//这里可以用矩阵乘法代替for循环                                               
        Eigen::Vector4f point;
        point << xyz_id_centriod.col(j), 1;
        float distanceCur = (point.dot(plane)) / plane.block<3, 1>(0, 0).norm();
        dataMin = dataMin > distanceCur ? distanceCur : dataMin;
        dataMax = dataMax < distanceCur ? distanceCur : dataMax;
    }

    std::cout << "LS用时：" << time.toc() << "ms" << endl;
    return dataMax - dataMin;
}

本文链接：https://blog.csdn.net/weixin_41674673/article/details/135866043

原作者删帖不实内容删帖广告或垃圾文章投诉

智能推荐

使用nginx解决浏览器跨域问题_nginx不停的xhr-程序员宅基地

文章浏览阅读1k次。通过使用ajax方法跨域请求是浏览器所不允许的，浏览器出于安全考虑是禁止的。警告信息如下：不过jQuery对跨域问题也有解决方案，使用jsonp的方式解决，方法如下：$.ajax({ async:false, url: 'http://www.mysite.com/demo.do', // 跨域URL ty..._nginx不停的xhr

在 Oracle 中配置 extproc 以访问 ST_Geometry-程序员宅基地

文章浏览阅读2k次。关于在 Oracle 中配置 extproc 以访问 ST_Geometry，也就是我们所说的使用空间SQL 的方法，官方文档链接如下。http://desktop.arcgis.com/zh-cn/arcmap/latest/manage-data/gdbs-in-oracle/configure-oracle-extproc.htm其实简单总结一下，主要就分为以下几个步骤。..._extproc

Linux C++ gbk转为utf-8_linux c++ gbk->utf8-程序员宅基地

文章浏览阅读1.5w次。linux下没有上面的两个函数，需要使用函数 mbstowcs和wcstombsmbstowcs将多字节编码转换为宽字节编码wcstombs将宽字节编码转换为多字节编码这两个函数，转换过程中受到系统编码类型的影响，需要通过设置来设定转换前和转换后的编码类型。通过函数setlocale进行系统编码的设置。linux下输入命名locale -a查看系统支持的编码_linux c++ gbk->utf8

IMP-00009: 导出文件异常结束-程序员宅基地

文章浏览阅读750次。今天准备从生产库向测试库进行数据导入，结果在imp导入的时候遇到“ IMP-00009:导出文件异常结束” 错误，google一下，发现可能有如下原因导致imp的数据太大，没有写buffer和commit两个数据库字符集不同从低版本exp的dmp文件，向高版本imp导出的dmp文件出错传输dmp文件时，文件损坏解决办法：imp时指定..._imp-00009导出文件异常结束

python程序员需要深入掌握的技能_Python用数据说明程序员需要掌握的技能-程序员宅基地

文章浏览阅读143次。当下是一个大数据的时代，各个行业都离不开数据的支持。因此，网络爬虫就应运而生。网络爬虫当下最为火热的是Python，Python开发爬虫相对简单，而且功能库相当完善，力压众多开发语言。本次教程我们爬取前程无忧的招聘信息来分析Python程序员需要掌握那些编程技术。首先在谷歌浏览器打开前程无忧的首页，按F12打开浏览器的开发者工具。浏览器开发者工具是用于捕捉网站的请求信息，通过分析请求信息可以了解请..._初级python程序员能力要求

Spring @Service生成bean名称的规则（当类的名字是以两个或以上的大写字母开头的话，bean的名字会与类名保持一致）_@service beanname-程序员宅基地

文章浏览阅读7.6k次，点赞2次，收藏6次。@Service标注的bean，类名：ABDemoService查看源码后发现，原来是经过一个特殊处理：当类的名字是以两个或以上的大写字母开头的话，bean的名字会与类名保持一致public class AnnotationBeanNameGenerator implements BeanNameGenerator { private static final String C..._@service beanname

随便推点

二叉树的各种创建方法_二叉树的建立-程序员宅基地

文章浏览阅读6.9w次，点赞73次，收藏463次。1.前序创建#include<stdio.h>#include<string.h>#include<stdlib.h>#include<malloc.h>#include<iostream>#include<stack>#include<queue>using namespace std;typed_二叉树的建立

解决asp.net导出excel时中文文件名乱码_asp.net utf8 导出中文字符乱码-程序员宅基地

文章浏览阅读7.1k次。在Asp.net上使用Excel导出功能，如果文件名出现中文，便会以乱码视之。解决方法： fileName = HttpUtility.UrlEncode(fileName, System.Text.Encoding.UTF8);_asp.net utf8 导出中文字符乱码

笔记-编译原理-实验一-词法分析器设计_对pl/0作以下修改扩充。增加单词-程序员宅基地

文章浏览阅读2.1k次，点赞4次，收藏23次。第一次实验词法分析实验报告设计思想词法分析的主要任务是根据文法的词汇表以及对应约定的编码进行一定的识别，找出文件中所有的合法的单词，并给出一定的信息作为最后的结果，用于后续语法分析程序的使用；本实验针对 PL/0 语言的文法、词汇表编写一个词法分析程序，对于每个单词根据词汇表输出： (单词种类, 单词的值) 二元对。词汇表：种别编码单词符号助记符0beginb..._对pl/0作以下修改扩充。增加单词

android adb shell 权限,android adb shell权限被拒绝-程序员宅基地

文章浏览阅读773次。我在使用adb.exe时遇到了麻烦.我想使用与bash相同的adb.exe shell提示符,所以我决定更改默认的bash二进制文件(当然二进制文件是交叉编译的,一切都很完美)更改bash二进制文件遵循以下顺序> adb remount> adb push bash / system / bin /> adb shell> cd / system / bin> chm..._adb shell mv 权限

投影仪-相机标定_相机-投影仪标定-程序员宅基地

文章浏览阅读6.8k次，点赞12次，收藏125次。1. 单目相机标定引言相机标定已经研究多年，标定的算法可以分为基于摄影测量的标定和自标定。其中，应用最为广泛的还是张正友标定法。这是一种简单灵活、高鲁棒性、低成本的相机标定算法。仅需要一台相机和一块平面标定板构建相机标定系统，在标定过程中，相机拍摄多个角度下（至少两个角度，推荐10~20个角度）的标定板图像（相机和标定板都可以移动），即可对相机的内外参数进行标定。下面介绍张氏标定法（以下也这么称呼）的原理。原理相机模型和单应矩阵相机标定，就是对相机的内外参数进行计算的过程，从而得到物体到图像的投影_相机-投影仪标定

Wayland架构、渲染、硬件支持-程序员宅基地

文章浏览阅读2.2k次。文章目录Wayland 架构Wayland 渲染Wayland的硬件支持简述：　翻译一篇关于和 wayland 有关的技术文章, 其英文标题为Wayland Architecture .Wayland 架构若是想要更好的理解 Wayland 架构及其与 X (X11 or X Window System) 结构；一种很好的方法是将事件从输入设备就开始跟踪, 查看期间所有的屏幕上出现的变化。这就是我们现在对 X 的理解。内核是从一个输入设备中获取一个事件，并通过 evdev 输入_wayland