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;
}


版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/weixin_41674673/article/details/135866043

智能推荐

while循环&CPU占用率高问题深入分析与解决方案_main函数使用while(1)循环cpu占用99-程序员宅基地

文章浏览阅读3.8k次,点赞9次,收藏28次。直接上一个工作中碰到的问题,另外一个系统开启多线程调用我这边的接口,然后我这边会开启多线程批量查询第三方接口并且返回给调用方。使用的是两三年前别人遗留下来的方法,放到线上后发现确实是可以正常取到结果,但是一旦调用,CPU占用就直接100%(部署环境是win server服务器)。因此查看了下相关的老代码并使用JProfiler查看发现是在某个while循环的时候有问题。具体项目代码就不贴了,类似于下面这段代码。​​​​​​while(flag) {//your code;}这里的flag._main函数使用while(1)循环cpu占用99

【无标题】jetbrains idea shift f6不生效_idea shift +f6快捷键不生效-程序员宅基地

文章浏览阅读347次。idea shift f6 快捷键无效_idea shift +f6快捷键不生效

node.js学习笔记之Node中的核心模块_node模块中有很多核心模块,以下不属于核心模块,使用时需下载的是-程序员宅基地

文章浏览阅读135次。Ecmacript 中没有DOM 和 BOM核心模块Node为JavaScript提供了很多服务器级别,这些API绝大多数都被包装到了一个具名和核心模块中了,例如文件操作的 fs 核心模块 ,http服务构建的http 模块 path 路径操作模块 os 操作系统信息模块// 用来获取机器信息的var os = require('os')// 用来操作路径的var path = require('path')// 获取当前机器的 CPU 信息console.log(os.cpus._node模块中有很多核心模块,以下不属于核心模块,使用时需下载的是

数学建模【SPSS 下载-安装、方差分析与回归分析的SPSS实现(软件概述、方差分析、回归分析)】_化工数学模型数据回归软件-程序员宅基地

文章浏览阅读10w+次,点赞435次,收藏3.4k次。SPSS 22 下载安装过程7.6 方差分析与回归分析的SPSS实现7.6.1 SPSS软件概述1 SPSS版本与安装2 SPSS界面3 SPSS特点4 SPSS数据7.6.2 SPSS与方差分析1 单因素方差分析2 双因素方差分析7.6.3 SPSS与回归分析SPSS回归分析过程牙膏价格问题的回归分析_化工数学模型数据回归软件

利用hutool实现邮件发送功能_hutool发送邮件-程序员宅基地

文章浏览阅读7.5k次。如何利用hutool工具包实现邮件发送功能呢?1、首先引入hutool依赖<dependency> <groupId>cn.hutool</groupId> <artifactId>hutool-all</artifactId> <version>5.7.19</version></dependency>2、编写邮件发送工具类package com.pc.c..._hutool发送邮件

docker安装elasticsearch,elasticsearch-head,kibana,ik分词器_docker安装kibana连接elasticsearch并且elasticsearch有密码-程序员宅基地

文章浏览阅读867次,点赞2次,收藏2次。docker安装elasticsearch,elasticsearch-head,kibana,ik分词器安装方式基本有两种,一种是pull的方式,一种是Dockerfile的方式,由于pull的方式pull下来后还需配置许多东西且不便于复用,个人比较喜欢使用Dockerfile的方式所有docker支持的镜像基本都在https://hub.docker.com/docker的官网上能找到合..._docker安装kibana连接elasticsearch并且elasticsearch有密码

随便推点

Python 攻克移动开发失败!_beeware-程序员宅基地

文章浏览阅读1.3w次,点赞57次,收藏92次。整理 | 郑丽媛出品 | CSDN(ID:CSDNnews)近年来,随着机器学习的兴起,有一门编程语言逐渐变得火热——Python。得益于其针对机器学习提供了大量开源框架和第三方模块,内置..._beeware

Swift4.0_Timer 的基本使用_swift timer 暂停-程序员宅基地

文章浏览阅读7.9k次。//// ViewController.swift// Day_10_Timer//// Created by dongqiangfei on 2018/10/15.// Copyright 2018年 飞飞. All rights reserved.//import UIKitclass ViewController: UIViewController { ..._swift timer 暂停

元素三大等待-程序员宅基地

文章浏览阅读986次,点赞2次,收藏2次。1.硬性等待让当前线程暂停执行,应用场景:代码执行速度太快了,但是UI元素没有立马加载出来,造成两者不同步,这时候就可以让代码等待一下,再去执行找元素的动作线程休眠,强制等待 Thread.sleep(long mills)package com.example.demo;import org.junit.jupiter.api.Test;import org.openqa.selenium.By;import org.openqa.selenium.firefox.Firefox.._元素三大等待

Java软件工程师职位分析_java岗位分析-程序员宅基地

文章浏览阅读3k次,点赞4次,收藏14次。Java软件工程师职位分析_java岗位分析

Java:Unreachable code的解决方法_java unreachable code-程序员宅基地

文章浏览阅读2k次。Java:Unreachable code的解决方法_java unreachable code

标签data-*自定义属性值和根据data属性值查找对应标签_如何根据data-*属性获取对应的标签对象-程序员宅基地

文章浏览阅读1w次。1、html中设置标签data-*的值 标题 11111 222222、点击获取当前标签的data-url的值$('dd').on('click', function() { var urlVal = $(this).data('ur_如何根据data-*属性获取对应的标签对象

推荐文章

热门文章

相关标签