点云配准5 -辅助知识 最小二乘法代码实现拟合曲线（C++）
2024-05-20 21:38:28
详细可观看： 计算机视觉（本科） 北京邮电大学 鲁鹏 清晰完整合集_哔哩哔哩_bilibili
大约在45分钟左右
一、原理讲解
通过实验获得一些列的观测数值(假设为三个)：
其每个样本观测值对应的精确值为：
这里假设其观测值对应的准确值为：
上面矩阵计算公式可以等价于：
其误差计算公式：
其平方误差计算公式：
由于这是误差公式关于的平方公式，所以根据要达到误差最小，既是极点，对其求导，令其等于0：
可知：
此时，系数就找到了，带入就可：
最小二乘的难题：
1、如果拟合的曲线为垂直x轴的直线，那么就不存在y，x与y之间没有对应关系（就是斜率不存在），无法利用最小二乘拟合曲线。
2、这个对于旋转不行
权最小二乘（x,y一起考虑）：之前是考虑的两点之间同一点x对应的y距离之差，比如说下图的d2,现在考虑的是点到直线的距离，对应下图d1距离。
详细推导，可以看看上面给的链接。
二、代码实现
2.1 最小二乘拟合正弦函数
代码复现的统计学第一章-最小二乘拟合正弦函数，正则化，其官方python代码为：
#coding:utf-8
import numpy as np
import scipy as sp
from scipy.optimize import leastsq
import matplotlib.pyplot as plt
# 目标函数
def real_func(x):return np.sin(2*np.pi*x)# 多项式
def fit_func(p, x):f = np.poly1d(p)# print('f=',f)return f(x)# 残差
def residuals_func(p, x, y):ret = fit_func(p, x) - yreturn ret# 十个点
x = np.linspace(0, 1, 10)
x_points = np.linspace(0, 1, 1000)
# 加上正态分布噪音的目标函数的值
y_ = real_func(x)
y = [np.random.normal(0, 0.1) + y1 for y1 in y_]def fitting(M=0):"""M 为 多项式的次数"""# 随机初始化多项式参数p_init = np.random.rand(M + 1)# 最小二乘法p_lsq = leastsq(residuals_func, p_init, args=(x, y))print('Fitting Parameters:', p_lsq[0])## 可视化plt.plot(x_points, real_func(x_points), label='real')plt.plot(x_points, fit_func(p_lsq[0], x_points), label='fitted curve')plt.plot(x, y, 'bo', label='noise')plt.legend()plt.show()return p_lsq
# M=0
p_lsq_0 = fitting(M=0)
# M=1
p_lsq_1 = fitting(M=1)
# M=3
p_lsq_3 = fitting(M=3)
# M=9
p_lsq_9 = fitting(M=9)其运行结果图：
2.2 c++实现拟合正弦曲线（对应上诉代码M = 10）
头函数：
# include<iostream>
# include<Eigen/Dense>
# include<math.h>
# include<Eigen/Eigenvalues>
using namespace std;
using namespace Eigen;生成原理所讲矩阵w,x,yt
//生成观测值
MatrixXd x(20, 10);
//目标值
MatrixXd y(20, 1);
//权重值
MatrixXd w(10, 1) ;计算误差函数（）：
MatrixXd errCount(MatrixXd y_p, MatrixXd y_t)
{MatrixXd err = (y_p - y_t);MatrixXd myerr = err.transpose() * err;return myerr;}关键部分（原理）实现：
MatrixXd leastsq(MatrixXd y_p, MatrixXd y_t, MatrixXd x, MatrixXd w)
{MatrixXd err = errCount(y_p, y_t);if (err.sum() > 0.1){w = (x.transpose() * x).inverse() * x.transpose() * y;y_p = x * w;err = errCount(y_p, y_t);cout << err.sum() << endl;}return w;
}由于官方函数，说明当M=10(多项式的系数)效果最好，固然复现构造函数（只有一个观测值x）:
for (int i = 0; i < 20; i++){ x(i, 9) = 1;x(i, 8)=0.25 + sum;x(i, 7) = x(i, 8) * x(i, 8);x(i, 6) = x(i, 8) * x(i, 8) * x(i, 8);x(i, 5) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);x(i, 4) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);x(i, 3) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);x(i, 2) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);x(i, 1) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8)*x(i, 8);x(i, 0) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8)*x(i, 8)*x(i, 8);y(i, 0) = sin(x(i,8)*3.14);sum = sum + 0.25;}w:0 0 0 0 0 0 0 0 0 13.8147e-06 1.52588e-05 6.10352e-05 0.000244141 0.000976562 0.00390625 0.015625 0.0625 0.25 10.00195312 0.00390625 0.0078125 0.015625 0.03125 0.0625 0.125 0.25 0.5 10.0750847 0.100113 0.133484 0.177979 0.237305 0.316406 0.421875 0.5625 0.75 11 1 1 1 1 1 1 1 1 17.45058 5.96046 4.76837 3.8147 3.05176 2.44141 1.95312 1.5625 1.25 138.4434 25.6289 17.0859 11.3906 7.59375 5.0625 3.375 2.25 1.5 1153.937 87.9639 50.2651 28.7229 16.4131 9.37891 5.35938 3.0625 1.75 1512 256 128 64 32 16 8 4 2 11477.89 656.841 291.929 129.746 57.665 25.6289 11.3906 5.0625 2.25 13814.7 1525.88 610.352 244.141 97.6562 39.0625 15.625 6.25 2.5 18994.86 3270.86 1189.4 432.51 157.276 57.1914 20.7969 7.5625 2.75 119683 6561 2187 729 243 81 27 9 3 140453 12447.1 3829.87 1178.42 362.591 111.566 34.3281 10.5625 3.25 178815.6 22518.8 6433.93 1838.27 525.219 150.062 42.875 12.25 3.5 1146650 39106.6 10428.4 2780.91 741.577 197.754 52.7344 14.0625 3.75 1262144 65536 16384 4096 1024 256 64 16 4 1452377 106442 25045.1 5892.96 1386.58 326.254 76.7656 18.0625 4.25 1756681 168151 37366.9 8303.77 1845.28 410.062 91.125 20.25 4.5 1
1.23096e+06 259149 54557.6 11485.8 2418.07 509.066 107.172 22.5625 4.75 1yt:0 0.706825 1 0.707951 0.00159265 -0.705698 -0.999997 -0.709075 -0.0031853 0.704568 0.999992 0.710197 0.00477794 -0.703437 -0.999984 -0.711317 -0.00637057 0.702304 0.999974 0.712436
原始y_p:1 1.33333 1.99805 3.77475 10 33.2529 113.33 357.853 1023 2659.41 6357.16 14134.2 29524 58431.6 110341 199977 349525 591569 972875 1.55921e+06变换过后：
精确值0 0.706825 1 0.707951 0.00159265 -0.705698 -0.999997 -0.709075 -0.0031853 0.704568 0.999992 0.710197 0.00477794 -0.703437 -0.999984 -0.711317 -0.00637057 0.702304 0.999974 0.712436
变换后的y_p:0.0103961 0.658876 1.06156 0.724083 -0.0521667 -0.743825 -0.969841 -0.649122 0.0135521 0.657116 0.942181 0.709952 0.0648353 -0.655788 -1.02993 -0.781023 -0.000910797 0.785843 0.94112 0.7246962.3 完整代码
# include<iostream>
# include<Eigen/Dense>
# include<math.h>
# include<Eigen/Eigenvalues>
using namespace std;
using namespace Eigen;
//生成观测值
MatrixXd x(20, 10);
//目标值
MatrixXd y(20, 1);
//权重值
MatrixXd w(10, 1) ;MatrixXd errCount(MatrixXd y_p, MatrixXd y_t)
{MatrixXd err = (y_p - y_t);MatrixXd myerr = err.transpose() * err;return myerr;}
MatrixXd leastsq(MatrixXd y_p, MatrixXd y_t, MatrixXd x, MatrixXd w)
{MatrixXd err = errCount(y_p, y_t);if (err.sum() > 0.1){w = (x.transpose() * x).inverse() * x.transpose() * y;y_p = x * w;err = errCount(y_p, y_t);cout << err.sum() << endl;}return w;
}int main()
{float sum=-0.25;//给观测值和准确值赋值for (int i = 0; i < 20; i++){ x(i, 9) = 1;x(i, 8)=0.25 + sum;x(i, 7) = x(i, 8) * x(i, 8);x(i, 6) = x(i, 8) * x(i, 8) * x(i, 8);x(i, 5) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);x(i, 4) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);x(i, 3) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);x(i, 2) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);x(i, 1) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8)*x(i, 8);x(i, 0) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8)*x(i, 8)*x(i, 8);y(i, 0) = sin(x(i,8)*3.14);sum = sum + 0.25;}cout << "w:" << endl;cout << x << endl;cout << "yt:" << endl;cout << y.transpose() << endl;w << 1, 1, 1, 1, 1, 1, 1, 1, 1, 1;cout << "原始y_p:" << endl;cout << (x * w).transpose() << endl;MatrixXd y_p = x * w;w = leastsq(y_p, y, x, w);cout << "精确值" << endl;cout << y.transpose() << endl;cout << "变换后的y_p:" << endl;cout << (x * w).transpose() << endl;}
3 、总结
根据上诉公式，好像只需要变换一次，有点奇怪。实验证明，也只变了一次，误差就很小了。
点云配准5 -辅助知识 最小二乘法代码实现拟合曲线（C++）相关推荐
- 该如何学习三维点云配准的相关知识？
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 知乎用户 上海交通大学 电子与信息博士在读 点云配准综述 本文意在 ...
- 点云配准（二）— python open3d ICP方法
上一节中介绍了点云配准的基础知识.本节将采用python open3d来进行点云配准. open3d安装和点云配准介绍,请参考: Open3d读写ply点云文件_Coding的叶子的博客-CSDN博客 ...
- 点云配准方法原理（NDT、ICP）
配准是点云处理中的一个基础问题,众多学者此问题进行了广泛而深入的研究,也出现了一系列优秀成熟的算法,在三维建模.自动驾驶等领域发挥着重要的作用. 本文主要介绍粗配准NDT (Normal Distri ...
- 一分钟详解点云配准ICP方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者:丁洪凯 链接:https://zhuanlan.zhihu.com/p/107218828 本文 ...
- 点云配准算法ICP及其各种变体
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨流川峰 来源丨深蓝AI 介绍 点云配准(Point Cloud Registration)算法指 ...
- 点云配准的传统算法ICP与NDT概述
公众号致力于分享点云处理,SLAM,三维视觉,高精地图相关的文章与技术,欢迎各位加入我们,一起交流一起进步,有兴趣的可联系微信:920177957.本文来自点云PCL博主的分享,未经作者允许请勿转载, ...
- 快速精确的体素GICP三维点云配准算法
标题:Voxelized GICP for Fast and Accurate 3D Point Cloud Registration 作者:Kenji Koide, Masashi Yokozuka ...
- 针对地图可压缩性的点云配准方法评估（IROS 2021）
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨Penny@知乎 来源丨https://zhuanlan.zhihu.com/p/2009241 ...
- CVPR 2021| 基于深度图匹配的鲁棒点云配准框架
Robust Point Cloud Registration Framework Based on Deep Graph Matching 代码地址:在公众号「3D视觉工坊」,后台回复「鲁棒点云」, ...
最新文章
- Python3中内置函数callable介绍
- 态势“知”多少，点开就知道
- 挑战杯科展上的智能车作品
- selenium+java初级学习笔记之单个元素定位
- 关于注册规划师的点点碎碎
- html演示 用鼠标画记号,html怎么用鼠标画出一条直线，鼠标移动时候要能看到线条...
- 句句真研—每日长难句打卡Day4
- asdm如何管理ips模块_自动驾驶深受高精度定位困扰，ST如何应对挑战？
- kitti数据集介绍论文Vision meets Robotics: The KITTI Dataset
- 第九届蓝桥杯单片机省赛试题
- 语言技能c1,从0开始学法语，20个月考下DALF C1，我如何立竿见影学语言？
- java溢出 事件触发_Java各种溢出
- UT单元测试总结基础篇
- windows自带日语输入法快捷键
- 微信预览wx.previewImage黑屏
- 网络游戏怎么样推广引流，游戏推广怎么做引流
- VScode+Unity3D的配置
- 上行物理信道 PUSCH
- 嵌入式开发培训去哪？参加培训班你被坑了？
- Android -- Wifi启动流程分析