离散点曲率计算方法c语言,自动驾驶(七十三)---------离散点计算曲率
在自动驾驶中,很常见的一个问题是通过离散点计算曲率,奇怪的是网上居然找不到我需要的资料,也许是我描述的不对,总之找了很久都不是很满意,所以干脆自己来总结一下吧。
网上也不是找不到资料,但是大部分都是这样:
(1)通过
曲率公式,首先你要知道曲线的表达式,然后代入公式计算。
(2)离散点也需要先polyfit拟合成多项式,再利用上面的公式计算曲率。
(3)在matlab中,用csape()对离散点进行Spline插值,然后用fnder()对得到曲线求导,最后用fnval()对导数求值即可。
(4)通过标量
公式,计算曲率。
(5)三个点外接圆:
但是三个点计算,不好推广。
总之以上的一些方法都不直接,拐弯抹角计算曲率,曲率直接理解就是角度变化率,近似对应于曲率半径的倒数,利用一串点鲁棒的计算对应的曲率就应该很简单才对,不要什么拟合多项式再代公式求曲率,简直是隔靴搔痒、舍本逐末。这里我总结一种直观的方法,抛砖引玉:
1. 离散点拟合圆
曲率的倒数就是半径,那我先拟合圆,知道半径也就知道曲率了,圆的拟合可以使多个点,拟合也有足够的鲁棒性。
用圆的展开公式:
。圆心为
,圆的半径就是
。
误差方程为:
对于自变量a、b、c求偏导为0:
得到:
为了直观和看代码方便,我们假设
上式可以描述为:
通过矩阵知识可以解出:
void FitCenterByLeastSquares(std::vector mapPoint, Point3D ¢erP, double &radius) {
double sumX = 0, sumY = 0;
double sumXX = 0, sumYY = 0, sumXY = 0;
double sumXXX = 0, sumXXY = 0, sumXYY = 0, sumYYY = 0;
int pCount = mapPoint.size();
for (int i = 0; i
sumX += mapPoint[i].x;
sumY += mapPoint[i].y;
sumXX += pow(mapPoint[i].x,2);
sumYY += pow(mapPoint[i].y,2);
sumXY += mapPoint[i].x * mapPoint[i].y;
sumXXX += pow(mapPoint[i].x,3);
sumXXY += pow(mapPoint[i].x,2) * mapPoint[i].y;
sumXYY += mapPoint[i].x * pow(mapPoint[i].y,2);
sumYYY += pow(mapPoint[i].y,3);
}
double M1 = pCount * sumXY - sumX * sumY;
double M2 = pCount * sumXX - sumX * sumX;
double M3 = pCount * (sumXXX + sumXYY) - sumX * (sumXX + sumYY);
double M4 = pCount * sumYY - sumY * sumY;
double M5 = pCount * (sumYYY + sumXXY) - sumY * (sumXX + sumYY);
double a = (M1 * M5 - M3 * M4) / (M2 * M4 - M1 * M1);
double b = (M1 * M3 - M2 * M5) / (M2 * M4 - M1 * M1);
double c = -(a * sumX + b * sumY + sumXX + sumYY) / pCount;
//圆心XY 半径
double xCenter = -0.5 * a;
double yCenter = -0.5 * b;
radius = 0.5 * sqrt(a * a + b * b - 4 * c);
centerP.x = xCenter;
centerP.y = yCenter;
}
2. 计算正负号
通过上面我们计算出圆的半径,倒数就是曲率,那么曲率的正负号呢?这里我们也需要一种方法优雅的计算它的正负号,不然有失风度,目前我能给出的方法是直线与圆心的关系:
计算起始点与终点的直线,判断该直线与圆心的关系从而判断曲率的正负号,又因为前面已经计算了很多最小二乘法的中间变量,要是不用也可惜了,干脆拟合直线算了:
类似的误差方程:
偏导为0得到:
和
联立解方程得到:
最后把圆心代入方程判断正负即可。
这里给出新加的代码,只需要在原来的基础上只要加3行代码即可,基本上还算优雅吧:
//计算曲率正负号
a = (sumXY-((sumX*sumY)/pCount))/(sumXX-(sumX*sumX/pCount));
b = (sumY-a*sumX)/pCount;
centerP.z = (xCenter*a+b
离散点曲率计算方法c语言,自动驾驶(七十三)---------离散点计算曲率相关推荐
- 自动驾驶计算系统的多种计算架构---分布式计算架构、中心式计算架构和混合式计算架构
编者按:在走完5个月漫长的收购流程之后,英特尔公司(Intel)150亿美元收购以色列ADAS技术供应商Mobileye的收购案终于在北京时间8月9日完成.不得不说,半导体产业(包括汽车半导体)的兼并 ...
- 空间曲线曲率算法c语言,第一章第四节空间曲线曲率计算公式及推导.doc
第一章第四节空间曲线曲率计算公式及推导 空间曲线的曲率定义及 计算公式 引理 设是单位圆周上的向量,即, 设与之间的夹角记为,则有 . 证明 因为 , 所以 . (用解等腰三角形或用余弦定理,得 .) ...
- 自动驾驶:车辆转弯半径计算
参考: https://blog.csdn.net/zghforever/article/details/103323986 https://zhuanlan.zhihu.com/p/10383415 ...
- 自动驾驶道路曲率计算
自动驾驶系列 车道曲率和中心点偏离距离计算 文章目录 自动驾驶系列 目标 一.曲率的介绍 圆的曲率 曲线的曲率 二.实现 1.计算曲率半径的方法,代码实现如下: 总结 目标 知道车道曲率计算的方法 知 ...
- Imagination推最强多核GPU系列!AI算力达24 TOPS,支持自动驾驶和数据中心
芯东西(公众号:aichip001)文 | 心缘 芯东西10月13日报道,刚刚,Imagination Technologies宣布推出又一大重磅新品--全新IMG B系列GPU IP. 这是其第一个 ...
- 自动驾驶之入门目录【转】
从零开始做自动驾驶定位(一): 开篇 - 知乎 从零开始做自动驾驶定位(二): 数据集 - 知乎 从零开始做自动驾驶定位(三): 软件框架 - 知乎 从零开始做自动驾驶定位(四): 前端里程计之初试 ...
- 超星未来发布新一代高级别自动驾驶车载计算平台
5月25日,由中国汽车工程学会.国家智能网联汽车创新中心主办的第八届国际智能网联汽车技术年会(以下称"CICV 2021")在北京亦创国际会展中心举办.超星未来联合创始人.首席技术 ...
- 自动驾驶LiDAR点云深度学习综述
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 / 导读 / 本文是滑铁卢大学CogDrive实验室和Geospatial Sensing and D ...
- 为什么说特斯拉研发自动驾驶AI芯片应该引起注意?
来源:36Kr 概要:对于特斯拉而言,研发这款芯片+配套算法本质上还是对率先将自动驾驶汽车商业化节点的争夺. 特斯拉Model 3的量产问题仍未彻底解决,CEO Elon Musk又抛出了自研自动驾驶 ...
最新文章
- easyUI显示echarts
- Python:变量与字符串
- python类高级用法_十.python面向对象高级用法
- mysql 读写分离 ,mysql_proxy实现
- dateFormat in DatePicker control Fiori - language 语言
- 正则高级用法-分组group+替换
- C/C++下载文件_上传文件
- addition过程 sgnb_5G NR接入优化问题排查思路
- 支付宝ios SDK官方下载页面
- html烟火源码,HTML5:烟火
- 黑马程序员全套Java教程_Java基础教程_目录
- indesign用于产品排班_怎样利用Indesign来进行高效的排版ID:30077776
- 备战软件设计大赛经验分享篇
- WinEdit如何修改字体大小
- WinEdit + CTex 打开论文模板出现乱码
- 裸机运行c语言,裸机_GPIO实验_C语言
- [论文阅读笔记38]基于多标准主动学习EMR实体识别方法
- 安庆集团-冲刺日志(第五天)
- 个人永久性免费-Excel催化剂功能第22波-Excel文件类型、密码批量修改,补齐PowerQuery短板...
- 如何在CentOS 8上安装Docker
热门文章
- 电脑开机后oracle中perl自启,PERL 引起Oracle数据库100% CPU
- spooling技术课程设计 linux环境,SPOOLING技术模拟实现 操作系统原理课程设计.doc
- where 和on的区别
- 支付宝小程序创建到发布
- java基础_集合框架
- 计算机感知 计算机神经网络,神经网络计算机,neural network computer,音标,读音,翻译,英文例句,英语词典...
- Html中控制文字的排版方向(左-右-上-下或者上-下-右-左)
- 智能车大赛电磁组------电感的测试
- autojs教程:果果山app脚本代码
- 智能制造技术基础复习内容全