⼀: 拟合⼀个平⾯：使⽤SVD分解，代码⾥⾯去吧
空间平⾯⽅程的⼀般表达式为：
Ax+By+Cz+D=0;
则有：
平⾯法向量为n=（A,B,C）.
第⼀种⽅法：对于空间中n个点（n3）
空间中的离散点得到拟合平⾯，其实这就是⼀个最优化的过程。即求这些点到某个平⾯距离最⼩和的问题。由此，我们知道⼀个先验消息，那就是该平⾯⼀定会过众散点的平均值。接着我们需要做的⼯作就是求这个平⾯的法向量。
根据协⽅差矩阵的SVD变换，最⼩奇异值对应的奇异向量就是平⾯的⽅向。
注意：这个⽅法是直接的计算⽅法，没办法解决数值计算遇到的病态矩阵问题.在公式转化代码之前必须
对空间点坐标进⾏近似归⼀化！
第⼆种⽅法：使⽤法线⽅法，对于空间中n个点（n3），若已获得点云法线
使⽤合适的⽅法剔除离点，计算点云的形⼼P；
若在已经获得法线的点云中，可以对法线进⾏剔除离散点之后，求取最⼩⽅差的均值，直接求得法线⽅向N( alpha, beta, theta )；
使⽤点法式描述三维平⾯；或者根据形⼼P和法线⽅向，计算出平⾯⽅程的⼀般式。
使⽤法线多次聚类：完成场景平⾯提取
使⽤法线两次聚类：第⼀次根据法线⽅向进⾏聚类，使⽤⼀个欧式距离约束，出⽅向接近的簇S(1)，这样得到的S(1)内的集合，每⼀类指向了⼤致相同的⽅向，但距离上并不⼀定接近；第⼆次，再次根据点云的空间位置进⾏聚类，对S(1)的每⼀簇内再次进⾏基于距离的聚类，出每⼀簇内位置接近的类别，这样再次对集合进⾏划分，得到的每⼀类⽅向⼤致相同，⽽位置较近，可以假设为⼀个平⾯的点。
此外，若考虑到平⾯密度要求，还可以再根据密度进⾏⼀次聚类，把密度较低的平⾯从集合中踢出去。
（2）：空间向量的旋转：
2-D绕原点旋转变换矩阵是：
[cosA sinA] [cosA -sinA]
[-sinA cosA] 或者 [sinA cosA]
2-D绕任意⼀点旋转变换矩阵是：
[x y 1] [1 0 0] [cosA sinA 0] [1 0 0] [x' y' -]
[0 1 0] x [0 1 0] x [-sinA cosA 0] x [0 1 0] = [- - -]
[0 0 1] [rtx rty 1] [0 0 1] [-rtx -rty 1] [- - -]
⼆：利⽤Ransac算法进⾏拟合
作者：王先荣原⽂链接:
本⽂翻译⾃，英⽂原⽂地址是：/wiki/ransac，如果您英语不错，建议您直接查看原⽂。
RANSAC是“RANdom SAmple Consensus（随机抽样⼀致）”的缩写。它可以从⼀组包含“局外点”的观测数据集中，通过迭代⽅式估计数学模型的参数。它是⼀种不确定的算法——它有⼀定的概率得出⼀个合理的结果；为了提⾼概率必须提⾼迭代次数。该算法最早由Fischler和Bolles于1981年提出。 RANSAC的基本假设是：
（1）数据由“局内点”组成，例如：数据的分布可以⽤⼀些模型参数来解释；（2）“局外点”是不能适应该模型的数据；（3）除此之外的数据属于噪声。 局外点产⽣的原因有：噪声的极值；错误的测量⽅法；对数据的错误假设。
RANSAC也做了以下假设：给定⼀组（通常很⼩的）局内点，存在⼀个可以估计模型参数的过程；⽽该模型能够解释或者适⽤于局内点。
本⽂内容定做三洋注塑机射咀头
1 ⽰例
2 概述
3 算法
4 参数
5 优点与缺点
6 应⽤
7 参考⽂献
8 外部链接
⼀、⽰例
⼀个简单的例⼦是从⼀组观测数据中出合适的2维直线。假设观测数据中包含局内点和局外点，其中局内点近似的被直线所通过，⽽局外点远离于直线。简单的最⼩⼆乘法不能到适应于局内点的直线，原因是最⼩⼆乘法尽量去适应包括局外点在内的所有点。相反，RANSAC能得出⼀个仅仅⽤局内点计算出模型，并且概率还⾜够⾼。但是，RANSAC并不能保证结果⼀定正确，为了保证算法有⾜够⾼的合理概率，我们必须⼩⼼的选择算法的参数。
左图：包含很多局外点的数据集右图：RANSAC到的直线（局外点并不影响结果）
⼆、概述
RANSAC算法的输⼊是⼀组观测数据，⼀个可以解释或者适应于观测数据的参数化模型，⼀些可信的参数。
RANSAC通过反复选择数据中的⼀组随机⼦集来达成⽬标。被选取的⼦集被假设为局内点，并⽤下述⽅法进⾏验证：
1.有⼀个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
2.⽤1中得到的模型去测试所有的其它数据，如果某个点适⽤于估计的模型，认为它也是局内点。
3.如果有⾜够多的点被归类为假设的局内点，那么估计的模型就⾜够合理。
4.然后，⽤所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
5.最后，通过估计局内点与模型的错误率来评估模型。
这个过程被重复执⾏固定的次数，每次产⽣的模型要么因为局内点太少⽽被舍弃，要么因为⽐现有的
模型更好⽽被选⽤。
三、算法
伪码形式的算法如下所⽰：
输⼊：
data —— ⼀组观测数据
model —— 适应于数据的模型
n —— 适⽤于模型的最少数据个数
k —— 算法的迭代次数
t —— ⽤于决定数据是否适应于模型的阀值
d —— 判定模型是否适⽤于数据集的数据数⽬
输出：
best_model —— 跟数据最匹配的模型参数（如果没有到好的模型，返回null）
best_consensus_set —— 估计出模型的数据点
best_error —— 跟数据相关的估计出的模型错误
iterations = 0
仿洞石涂料best_model = null
best_consensus_set = null
best_error = ⽆穷⼤
while ( iterations < k )
maybe_inliers = 从数据集中随机选择n个点
maybe_model = 适合于maybe_inliers的模型参数
consensus_set = maybe_inliers
for ( 每个数据集中不属于maybe_inliers的点）
if ( 如果点适合于maybe_model，且错误⼩于t ）
将点添加到consensus_set
if （ consensus_set中的元素数⽬⼤于d ）
已经到了好的模型，现在测试该模型到底有多好
better_model = 适合于consensus_set中所有点的模型参数
this_error = better_model究竟如何适合这些点的度量
if ( this_error < best_error )
我们发现了⽐以前好的模型，保存该模型直到更好的模型出现
成官宝
best_model = better_model
best_consensus_set = consensus_set
best_error = this_error
电子管功放电路增加迭代次数
返回 best_model, best_consensus_set, best_error
RANSAC算法的可能变化包括以下⼏种：
（1）如果发现了⼀种⾜够好的模型（该模型有⾜够⼩的错误率），则跳出主循环。这样可能会节约计算额外参数的时间。
（2）直接从maybe_model计算this_error，⽽不从consensus_set重新估计模型。这样可能会节约⽐较两种模型错误的时间，但可能会对噪声更敏感。
四、参数
我们不得不根据特定的问题和数据集通过实验来确定参数t和d。然⽽参数k（迭代次数）可以从理论结果推断。当我们从估计模型参数时，⽤p表⽰⼀些迭代过程中从数据集内随机选取出的点均为局内点的概率；此时，结果模型很可能有⽤，因此p也表征了算法产⽣有⽤结果的概率。⽤w表⽰每次从数据集中选取⼀个局内点的概率，如下式所⽰：
w = 局内点的数⽬ / 数据集的数⽬
通常情况下，我们事先并不知道w的值，但是可以给出⼀些鲁棒的值。假设估计模型需要选定n个点，w n是所有n个点均为局内点的概率；1 −w n是n个点中⾄少有⼀个点为局外点的概率，此时表明我们从数据集中估计出了⼀个不好的模型。 (1 −w n)k表⽰算法永远都不会选择到n个点均为局内点的概率，它和1-p相同。因此， 1 −p = (1 − w n)k
我们对上式的两边取对数，得出
值得注意的是，这个结果假设n个点都是独⽴选择的；也就是说，某个点被选定之后，它可能会被后续的迭代过程重复选定到。这种⽅法通常都不合理，由此推导出的k值被看作是选取不重复点的上限。例如，要从上图中的数据集寻适合的直线，RANSAC算法通常在每次迭代时选取2个点，计算通过这两点的直线maybe_model，要求这两点必须唯⼀。
为了得到更可信的参数，标准偏差或它的乘积可以被加到k上。k的标准偏差定义为：
五、优点与缺点
RANSAC的优点是它能鲁棒的估计模型参数。例如，它能从包含⼤量局外点的数据集中估计出⾼精度的参数。
RANSAC的缺点是它计算参数的迭代次数没有上限；如果设置迭代次数的上限，得到的结果可能不是最优的结果，甚⾄可能得到错误的结果。RANSAC只有⼀定的概率得到可信的模型，概率与迭代次数成正⽐。
RANSAC的另⼀个缺点是它要求设置跟问题相关的阀值。
RANSAC只能从特定的数据集中估计出⼀个模型，如果存在两个（或多个）模型，RANSAC不能到别的模型。
六、应⽤
RANSAC算法经常⽤于计算机视觉，例如同时求解相关问题与估计⽴体摄像机的基础矩阵。
三、点云匹配：ICP算法（Iterative Closest Point迭代最近点）
参考链接：：
ICP（Iterative Closest Point迭代最近点）算法是⼀种点集对点集配准⽅法，
如下图，假设PR（红⾊块）和RB（蓝⾊块）是两个点集，该算法就是计算怎么把PB平移旋转，使PB和PR尽量重叠，建⽴模型的Alignment模型。
（图1）
ICP是改进⾃对应点集配准算法的对应点集配准算法是假设⼀个理想状况，将⼀个模型点云数据X（如上图的PB）利⽤四元数旋转，并平移得到点云P（类似于上图的PR）。⽽对应点集配准算法主要就是怎么计算出qR和qT的，知道这两个就可以匹配点云了。
但是对应点集配准算法的前提条件是计算中的点云数据PB和PR的元素⼀⼀对应，这个条件在现实⾥因误差等问题，不太可能实线，所以就有了ICP算法（我们⽆从知道两个点集之间的匹配关系，只能认为距离最近的点为同⼀个，所以此⽅法为迭代最近点）。
水箅ICP算法是从源点云上的（PB）每个点先计算出⽬标点云（PR）的每个点的距离，使每个点和⽬标云的最近点匹配。
这样满⾜了对应点集配准算法的前提条件、每个点都有了对应的映射点，则可以按照对应点集配准算法计算，但因为这个是假设，所以需要重复迭代运⾏上述过程，直到均⽅差误差⼩于某个阀值。
也就是说每次迭代，整个模型是靠近⼀点，每次都重新最近点，然后再根据对应点集批准算法算⼀次，⽐较均⽅差误差，如果不满⾜就继续迭代。
ICP的求解⽅法：
把ICP⽅法看做⼀个点云位姿变换的过程，可以使⽤代数⽅法和⾮线性优化⽅法。
假设有两堆点云，分别记为两个集合X=x1,x2,...,x m和Y=y1,y2,...,y m（m并不总是等于n）。
ICP公式为：
1.SVD等代数⽅法
先构建误差矩阵，构建最⼩⼆乘问题，求使得误差平⽅和最⼩的点云旋转和位移R，T。
初始化估计：ICP发展了多年之后，当然有很多的⽅法来估计初始的R和t，PCL⾃⼰的函数 SampleC
onsensusInitalAlignment 函数以及TransformationEstimationSVD函数都可以得到较好的初始估计。
优化：得到初始化估计之后仍然存在误差问题，RANSAC之后，若已存在完全正确匹配，则可以再次求取旋转的essential矩阵，通过SVD分解得到最终旋转R和平移t。
2.⾮线性优化⽅法
RANSAC算法之后，去除掉外点之后。
使⽤位姿的代数变化转换构建⼀个误差项，在⾮线性优化过程中不停地迭代，⼀般能到极⼩值。
后记：
在去除外点，匹配已知的情况下，ICP的最⼩⼆乘问题总会得到⼀个最优解，即ICP的SVD⽅法总会有⼀个解析解。
（⼆）RANSAC算法与ICP算法对⽐（RANdom SAmple Consensus随机抽样⼀致）
它可以从⼀组包含“局外点”的观测数据集中，通过迭代⽅式估计数学模型的参数。它是⼀种不确定的算法——它有⼀定的概率得出⼀个合理的结果；为了提⾼概率必须提⾼迭代次数。该算法最早由Fischler和Bolles于1981年提出。
光看⽂字还是太抽象了，我们再⽤图描述
RANSAC的基本假设是：（1）数据由“局内点”组成，例如：数据的分布可以⽤⼀些模型参数来解释；（2）“局外点”是不能适应该模型的数据；（3）除此之外的数据属于噪声。
⽽下图⼆⾥⾯、蓝⾊部分为局内点，⽽红⾊部分就是局外点，⽽这个算法要算出的就是蓝⾊部分那个模型的参数
（图⼆）
RANSAC算法的输⼊是⼀组观测数据，⼀个可以解释或者适应于观测数据的参数化模型，⼀些可信的参数。
在上图⼆中左半部分灰⾊的点为观测数据，⼀个可以解释或者适应于观测数据的参数化模型我们可以在这个图定义为⼀条直线，如y=kx + b；
⼀些可信的参数指的就是指定的局内点范围。⽽k，和b就是我们需要⽤RANSAC算法求出来的
RANSAC通过反复选择数据中的⼀组随机⼦集来达成⽬标。被选取的⼦集被假设为局内点，并⽤下述⽅法进⾏验证：
1.有⼀个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
2.⽤1中得到的模型去测试所有的其它数据，如果某个点适⽤于估计的模型，认为它也是局内点。
3.如果有⾜够多的点被归类为假设的局内点，那么估计的模型就⾜够合理。
4.然后，⽤所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
5.最后，通过估计局内点与模型的错误率来评估模型。
这个过程被重复执⾏固定的次数，每次产⽣的模型要么因为局内点太少⽽被舍弃，要么因为⽐现有的模型更好⽽被选⽤。
这个算法⽤图⼆的例⼦说明就是先随机到内点，计算k1和b1，再⽤这个模型算其他内点是不是也满⾜y=k1x+b2，评估模型
再跟后⾯的两个随机的内点算出来的k2和b2⽐较模型评估值，不停迭代最后到最优点
我再⽤图⼀的模型说明⼀下ICP算法
（图1）
ICP算法的输⼊是⼀组观测数据，⼀个可以解释或者适应于观测数据的参数化模型，⼀些可信的参数。
模型对应的是空间中⼀个点云数据到另外⼀个点云数据的旋转以及平移。第⼀步随机得到的是⼀个点云中的点对作，利⽤其不变特征（两点距离，两点法向量夹⾓）作为哈希表的索引值搜索另⼀个点云中的⼀对对应点对，然后计算得到旋转及平移的参数值。
金属卤化物灯接线图
然后适⽤变换，到其他局内点，并在到局内点之后重新计算旋转及平移为下⼀个状态。
然后迭代上述过程，到最终的位置
其中观测数据就是PB，⼀个可以解释或者适应于观测数据的参数化模型是四元数旋转，并平移
可信的参数是两个点对的不变特征（两点距离，两点法向量夹⾓）
区别：
ICP算法在迭代的时候，点对是已经匹配的；RANSAC算法在迭代的时候，点匹配对是随着优化函数改变的。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议