2024年C C++最新RANSAC与圆柱拟合(2),腾讯C C++开发面试经验

img
img

网上学习资料一大堆,但如果学到的知识不成体系,遇到问题时只是浅尝辄止,不再深入研究,那么很难做到真正的技术提升。

需要这份系统化的资料的朋友,可以添加戳这里获取

一个人可以走的很快,但一群人才能走的更远!不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人,都欢迎加入我们的的圈子(技术交流、学习资源、职场吐槽、大厂内推、面试辅导),让我们一起学习成长!

点向式:(x-x0)/u =(y-y0)/v=(z-z0) /w ,过点(x0,y0,z0) ,且有方向向量(u,v,w)。

设空间一点为P(x0,y0,z0),在直线上找一点Q(x1,y1,z1),直线的方向向量为:S=(l,m,n),则d=|PQ叉乘S|/|S|,

理由:|PQ叉乘S|为一平行四边形的面积,|S|为其一边.故=|PQ叉乘S|/|S|为平行四边形的高.即为点到直线的距离。

2、空间平面:三个不在一条直线上的点确定一个平面

Ax+By+Cz+D=0,即ax+by+cz+1=0

四个未知数求出三个就够啦,这三个未知数都可以用第四个未知数来表示,假设第四个未知数是D.则求出来的三个未知数一般是:A=a*D;B=b*D;C=c*D;(D不等于0),最终有a*Dx+b*Dy+cDz+D=0,等式两边同除以D,得平面方程ax+by+cz+1=0;

将已知三个点的坐标分别用P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)表示。

设通过P1,P2,P3三点的平面方程为:Ax + By + Cz + D = 0。将P1(x1,y1,z1)点数值代入方程Ax + By + Cz + D = 0。

即可得到:Ax1 + By 1+ Cz1 + D = 0。化简得D = -(A * x1 + B * y1 + C * z1)。

则可以根据P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)三点坐标分别求得A、B、C的值,如下:

A = (y3 - y1)*(z3 - z1) - (z2 -z1)*(y2 - y1);

B = (x3 - x1)*(z2 - z1) - (x2 - x1)*(z3 - z1);

C = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);

又D = -(A * x1 + B * y1 + C * z1),所以可以求得D的值。

利用RANSAC拟合平面:

clc;clear all;close all;

%%%三维平面拟合
%%%生成随机数据
%内点
mu=[0 0 0];  %均值
S=[2 0 4;0 4 0;4 0 8];  %协方差
data1=mvnrnd(mu,S,300);   %产生200个高斯分布数据
%外点
mu=[2 2 2];
S=[8 1 4;1 8 2;4 2 8];  %协方差
data2=mvnrnd(mu,S,100);     %产生100个噪声数据
%合并数据
data=[data1',data2'];
iter = 1000; 

%%% 绘制数据点
 figure;plot3(data(1,:),data(2,:),data(3,:),'o');hold on; % 显示数据点
 number = size(data,2); % 总点数
 bestParameter1=0; bestParameter2=0; bestParameter3=0; % 最佳匹配的参数
 sigma = 1;
 pretotal=0;     %符合拟合模型的数据的个数

for i=1:iter
 %%% 随机选择三个点
     idx = randperm(number,3); 
     sample = data(:,idx); 

     %%%拟合直线方程 z=ax+by+c
     plane = zeros(1,3);
     x = sample(:, 1);
     y = sample(:, 2);
     z = sample(:, 3);

     a = ((z(1)-z(2))*(y(1)-y(3)) - (z(1)-z(3))*(y(1)-y(2)))/((x(1)-x(2))*(y(1)-y(3)) - (x(1)-x(3))*(y(1)-y(2)));
     b = ((z(1) - z(3)) - a * (x(1) - x(3)))/(y(1)-y(3));
     c = z(1) - a * x(1) - b * y(1);
     plane = [a b -1 c]

     mask=abs(plane*[data; ones(1,size(data,2))]);    %求每个数据到拟合平面的距离
     total=sum(mask<sigma);              %计算数据距离平面小于一定阈值的数据的个数

     if total>pretotal            %找到符合拟合平面数据最多的拟合平面
         pretotal=total;
         bestplane=plane;          %找到最好的拟合平面
    end  
 end
 %显示符合最佳拟合的数据
mask=abs(bestplane*[data; ones(1,size(data,2))])<sigma;    
hold on;
k = 1;
for i=1:length(mask)
    if mask(i)
        inliers(1,k) = data(1,i);
        inliers(2,k) = data(2,i);
        plot3(data(1,i),data(2,i),data(3,i),'r+');
        k = k+1;
    end
end

 %%% 绘制最佳匹配平面
 bestParameter1 = bestplane(1);
 bestParameter2 = bestplane(2);
 bestParameter3 = bestplane(4);
 xAxis = min(inliers(1,:)):max(inliers(1,:));
 yAxis = min(inliers(2,:)):max(inliers(2,:));
 [x,y] = meshgrid(xAxis, yAxis);
 z = bestParameter1 * x + bestParameter2 * y + bestParameter3;
 surf(x, y, z);
 title(['bestPlane:  z =  ',num2str(bestParameter1),'x + ',num2str(bestParameter2),'y + ',num2str(bestParameter3)]);

在编程中,我们知道三个空间点之后,可以用 [uu, dd, vv] = svd(A); plane = vv(:,end); plane = plane ./ plane(end); plane = plane’;来快速求解法向量,确定一个空间平面。

from:https://blog.csdn.net/u010128736/article/details/53422070

3、MATLAB中的圆柱拟合:初始模型参数表示为[x,y,z,dx,dy,dz,r]。其中(x,y,z)是圆柱中心轴上的一个点,(dx,dy,dz)指定轴的方向,r是半径。

  • 第一步:每次随机选两个点P0,P1,这两个点在拟合圆柱的表面上。用于拟合中心轴和半径:

输入是2*6的矩阵,每行代表一个样本,每行的前3列代表x,y,z,后三行代表该点的法向量。

function model = fitCylinder(points)
p1 = points(1,1:3);
n1 = points(1,4:6);
p2 = points(2,1:3);
n2 = points(2,4:6);
w = p1 + n1 - p2;

a = n1 * n1';%模
b = n1 * n2';%点乘
c = n2 * n2';%模
d = n1 * w';
e = n2 * w';
D = a * c - b * b;
if abs(D) < 1e-5
    s = 0;
    if b > c
        t = d / b;
    else
        t = e / c;
    end
else
    s = (b * e - c * d) / D;
    t = (a * e - b * d) / D;
end

% P0 is a point on the axis
p0 = p1 + n1 + s * n1;
% dp is a normalized vector for the direction of the axis
dp = p2 + t * n2 - p0;%(p2 + t * n2) - p0
dp = dp / norm(dp);

p1p0 = p1 - p0;
p2p1 = dp;
c = [p1p0(2)*p2p1(3) - p1p0(3)*p2p1(2), ...
     p1p0(3)*p2p1(1) - p1p0(1)*p2p1(3), ...
     p1p0(1)*p2p1(2) - p1p0(2)*p2p1(1)];%就是p1p0,p2p1两向量的叉乘,a*b*cos,模表示两向量围成的平行四边形的面积
% p2p1 is a unit vector, so the denominator is not needed ,p2p1是单位向量,模为1,
r = sqrt(sum(c.*c, 2));%即c的模(面积)就是高,也就是半径

model = [p0, dp, r];
  • 第二步:根据模型参数,然后求全体点云中的每个点P0到中心轴的距离。
function dis = evalCylinder(model, points)
p1p0 = [points(:,1)-model(1), points(:,2)-model(2), points(:,3)-model(3)];
p2p1 = model(4:6);
c = [p1p0(:,2)*p2p1(3) - p1p0(:,3)*p2p1(2), ...
     p1p0(:,3)*p2p1(1) - p1p0(:,1)*p2p1(3), ...
     p1p0(:,1)*p2p1(2) - p1p0(:,2)*p2p1(1)];
% p2p1 is a unit vector, so the denominator is not needed
D = sum(c.*c, 2);
dis = abs(sqrt(D) - model(7));#求得是误差error
  • 第三步:计算损失,通过总损失来衡量模型的拟合好坏

        % Evaluate model with truncated loss评估具有截断损失
        dis = evalFunc(modelParams, allPoints);
        dis(dis > threshold) = threshold;
        accDis = sum(dis);

实例:

points=[1,1,0,1,0,0;1,2,0,1,0,0]
p1 = points(1,1:3);
n1 = points(1,4:6);
p2 = points(2,1:3);
n2 = points(2,4:6);
w = p1 + n1 - p2;

a = n1 * n1';%模
b = n1 * n2';%点乘,标量
c = n2 * n2';%模
d = n1 * w';
e = n2 * w';
D = a * c - b * b;
if abs(D) < 1e-5
    s = 0;
    if b > c
        t = d / b;
    else
        t = e / c;
    end
else
    s = (b * e - c * d) / D;
    t = (a * e - b * d) / D;
end

% P0 is a point on the axis
p0 = p1 + n1 + s * n1;
% dp is a normalized vector for the direction of the axis
dp = p2 + t * n2 - p0;%(p2 + t * n2) - p0
dp = dp / norm(dp);

p1p0 = p1 - p0;
p2p1 = dp;
c = [p1p0(2)*p2p1(3) - p1p0(3)*p2p1(2), ...
     p1p0(3)*p2p1(1) - p1p0(1)*p2p1(3), ...
     p1p0(1)*p2p1(2) - p1p0(2)*p2p1(1)];%就是p1p0,p2p1两向量的叉乘,a*b*sin,模表示两向量围成的平行四边形的面积
% p2p1 is a unit vector, so the denominator is not needed ,p2p1是单位向量,模为1,
r = sqrt(sum(c.*c, 2));%求c的模(表示两向量围成的平行四边形的面积)因为这里p2p1是单位向量,所以面积就是高,也就是半径

model = [p0, dp, r];

不行,还是看不懂MATLAB到底是怎么拟合圆柱了,求助!!!会的请留言给我,谢谢

圆柱拟合:

由圆柱面的几何特性可得,圆柱面上的点到其轴线的距离恒等于半径 r0,其中,P 为圆柱面上任意一点,P0( x0,y0,z0) 为圆柱轴线上一点, ( a,b,c) 为圆柱轴线向量,r0为圆柱底圆半径。圆柱面上任意一点到其轴线的距离为半径 r0,即:

求出这七个参数,就可以唯一确定一个圆柱。

圆柱拟合步骤主要包括两步: 一是确定柱面模型参数初始值; 二是建立误差方程式求解参数值。本文算法结合主成分分析法与线性最小二乘法,确定圆 柱 轴 线 向 量 ( a,b,c) 、圆 柱 轴 线 上 一 点( x1,y1,z1) 、圆柱底圆半径 r 这七个柱面模型参数初始值,再建立改进误差方程式,求解参数。

本文柱面拟合方法详细步骤如下:

1、确定柱面模型参数初始值。

搜寻圆柱面上任意一点的若干邻近点,将这些点拟合成平面,得到的平面法向量单位化即该点的单位法向量。对圆柱面上每个点处理,得到每个点的单位法向量。将每个点的单位法向量看成点,将这些点拟合成平面,得到平面法向量,即圆柱轴线向量初始值 a0、b0、c0,该步骤使用了主成分分析法进行求解。(这一步和matlab源码很相似,matlab使用两个点,及它们的法向量来拟合圆柱。) 得到轴线后,对圆柱进行坐标转换,使圆柱轴线向量 ( a0,b0,c0) 变 换 为 平 行 Z 轴 的 向 量 ,这样圆柱面上的点的(x,y)坐标就是一个平面圆形,用他们拟合圆心,即( x1,y1,z1)和半径r.

2、建立误差方程。

将误差方程写成矩阵形式,要让V最小,接近0:

from:https://www.cnblogs.com/wangkundentisy/p/7505487.html

用最小二乘法求解, Ax=0的最小二乘解,即为求解A^T*A的最小特征值对应的特征向量。这是一个循环的迭代过程,每一次迭代过程中代入的初值都等于上一次的初值加上求出的 X 的改正值,当 X 的数值小到满足要求的精度时退出迭代。

代码:

for (int i = 0; i < size; i++) {
float x = cloud_in->points[i].x;
float y = cloud_in->points[i].y;
float z = cloud_in->points[i].z;
 
f0 = pow(x - x0, 2) + pow(y - y0, 2) + pow(z - z0, 2) - pow((a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0)), 2) - r0 * r0;
L(i, 0) = -f0;
B(i, 0) = (a0 * (a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0)) - (x - x0)) * 2;
B(i, 1) = (b0 * (a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0)) - (y - y0)) * 2;
B(i, 2) = (c0 * (a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0)) - (z - z0)) * 2;
B(i, 3) = -(a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0))*(x - x0) * 2;
B(i, 4) = -(a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0))*(y - y0) * 2;
B(i, 5) = -(a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0))*(z - z0) * 2;
B(i, 6) = -2*r0;
}

X = B.colPivHouseholderQr().solve(L);,求解 L=BX,用最小二乘法求解X。

PCL实现:

PCL库自带的圆柱模型拟合,由于在查找最佳圆柱面的过程中会过滤很多点,因此考虑利用最小二乘的模型来拟合最接近实际点云的一个圆柱面,code如下:

#include "pch.h"
#include <iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/point_cloud.h>
#include<pcl/segmentation/sac_segmentation.h>
#include<pcl/search/search.h>
#include<pcl/search/kdtree.h>
#include<pcl/features/normal_3d.h>
#include<pcl/common/common.h>
 
 
// use ransanc to fit cylinder
int fitCylinder(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out) {
	// Create segmentation object for cylinder segmentation and set all the parameters
	pcl::ModelCoefficients::Ptr coeffients_cylinder(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);
 
	//  Normals
	pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ>>(new pcl::search::KdTree<pcl::PointXYZ>);
	pcl::PointCloud<pcl::Normal>::Ptr normalsFilter(new pcl::PointCloud<pcl::Normal>);
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimator;
	normalEstimator.setSearchMethod(tree);
	normalEstimator.setInputCloud(cloud_in);
	normalEstimator.setKSearch(30);
	normalEstimator.compute(*normalsFilter);
	
	pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_CYLINDER);
	seg.setMethodType(pcl::SAC_RANSAC);
	//seg.setNormalDistanceWeight(0.1);
	seg.setNormalDistanceWeight(0.2);		// for coarse data
	seg.setMaxIterations(1000);
	seg.setDistanceThreshold(0.3);
	seg.setRadiusLimits(0, 6);				// radius is within 8 centimeters
	seg.setInputCloud(cloud_in);
	seg.setInputNormals(normalsFilter);
 
	// Perform segment
	seg.segment(*inliers_cylinder, *coeffients_cylinder);
	std::cerr << "Cylinder coefficient: " << *coeffients_cylinder << std::endl;
	
	// Ouput extracted cylinder
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud_in);
	extract.setIndices(inliers_cylinder);
	extract.filter(*cloud_out);
	pcl::PCDWriter wr;
	wr.write("Extract_Cylinder.pcd", *cloud_out);
 
	float sum_D = 0.0;
	float sum_Ave = 0.0;
	float x0 = coeffients_cylinder->values[0];
	float y0 = coeffients_cylinder->values[1];
	float z0 = coeffients_cylinder->values[2];
	float l = coeffients_cylinder->values[3];


![img](https://img-blog.csdnimg.cn/img_convert/266ea9fc07cee036c1eafe1615d802bf.png)
![img](https://img-blog.csdnimg.cn/img_convert/126234638569331549b0e34d09382e2b.png)

**既有适合小白学习的零基础资料,也有适合3年以上经验的小伙伴深入学习提升的进阶课程,涵盖了95%以上C C++开发知识点,真正体系化!**

**由于文件比较多,这里只是将部分目录截图出来,全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、讲解视频,并且后续会持续更新**

**[如果你需要这些资料,可以戳这里获取](https://bbs.csdn.net/topics/618668825)**

0 = coeffients_cylinder->values[1];
	float z0 = coeffients_cylinder->values[2];
	float l = coeffients_cylinder->values[3];


[外链图片转存中...(img-VMx0vtjZ-1715553464869)]
[外链图片转存中...(img-753MyzeO-1715553464869)]

**既有适合小白学习的零基础资料,也有适合3年以上经验的小伙伴深入学习提升的进阶课程,涵盖了95%以上C C++开发知识点,真正体系化!**

**由于文件比较多,这里只是将部分目录截图出来,全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、讲解视频,并且后续会持续更新**

**[如果你需要这些资料,可以戳这里获取](https://bbs.csdn.net/topics/618668825)**

  • 26
    点赞
  • 10
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值