非线性优化-高斯牛顿法

在SLAM领域，后端多采用基于非线性优化的方法，来优化位姿和地图点，其中高斯牛顿法的使用频率很高。

求解高斯牛顿法的核心公式：

其中 f 是误差函数，J是误差关于待优化变量的雅可比矩阵。 

其中H为海森矩阵（error相对于变量的二阶导数的近似 ），海森矩阵的特征向量表示了优化问题中的不同方向，海森矩阵的特征值可以提供关于优化问题在不同方向上的曲率大小的信息，在高斯-牛顿算法中，特征值可以用作步长（步幅）的参考。

但是在实际的计算中，JTJ却只有半正定性，因此可能会出现奇异矩阵或者病态的情况。在SLAM的非线性优化中使用高斯牛顿法求解时，会通过近似的H矩阵的特征值大小判断是否发生退化情况。

特征值和特征向量的几何意义：

矩阵特征值，奇异值分解_奇异值分解的特征矩阵-CSDN博客 

LIO-SAM中scan-to-map的非线性优化问题采用了高斯牛顿法：

    /*** scan-to-map优化* 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped* 公式推导：todo*/bool LMOptimization(int iterCount){// This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation// lidar <- camera      ---     camera <- lidar// x = z                ---     x = y// y = x                ---     y = z// z = y                ---     z = x// roll = yaw           ---     roll = pitch// pitch = roll         ---     pitch = yaw// yaw = pitch          ---     yaw = roll// lidar -> camerafloat srx = sin(transformTobeMapped[1]);float crx = cos(transformTobeMapped[1]);float sry = sin(transformTobeMapped[2]);float cry = cos(transformTobeMapped[2]);float srz = sin(transformTobeMapped[0]);float crz = cos(transformTobeMapped[0]);// 当前帧匹配特征点数太少int laserCloudSelNum = laserCloudOri->size();if (laserCloudSelNum < 50) {return false;}cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));PointType pointOri, coeff;// 遍历匹配特征点，构建Jacobian矩阵for (int i = 0; i < laserCloudSelNum; i++) {// lidar -> camera todo// 首先将当前点以及点到线（面）的单位向量转到相机系pointOri.x = laserCloudOri->points[i].y;pointOri.y = laserCloudOri->points[i].z;pointOri.z = laserCloudOri->points[i].x;// lidar -> cameracoeff.x = coeffSel->points[i].y;coeff.y = coeffSel->points[i].z;coeff.z = coeffSel->points[i].x;coeff.intensity = coeffSel->points[i].intensity;// in camera// 对Roll角的求导float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;float ary = ((cry*srx*srz - crz*sry)*pointOri.x + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x+ ((-cry*crz - srx*sry*srz)*pointOri.x + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;// lidar -> camera// 雅可比矩阵，【6，1】的矩阵，前3个是误差关于旋转的导数，后3个是误差关于平移的导数。matA.at<float>(i, 0) = arz;matA.at<float>(i, 1) = arx;matA.at<float>(i, 2) = ary;matA.at<float>(i, 3) = coeff.z;matA.at<float>(i, 4) = coeff.x;matA.at<float>(i, 5) = coeff.y;// 点到直线距离、平面距离，作为观测值matB.at<float>(i, 0) = -coeff.intensity;}// 构造JTJ（H）以及-JTe(b)矩阵，matAt是转置cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;// J^T·J·delta_x = -J^T·f 高斯牛顿，matX是增量待求解量。cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);// 首次迭代，检查近似Hessian矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0 todoif (iterCount == 0) {cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));// 对H进行特征值分解cv::eigen(matAtA, matE, matV); matV.copyTo(matV2);isDegenerate = false;float eignThre[6] = {100, 100, 100, 100, 100, 100};for (int i = 5; i >= 0; i--) {// 特征值从小到大遍历，如果小于阈值就认为是退化。（特征值比较小，可能是一个零空间，无法得到最优估计？属于一个退化环境）if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}matP = matV.inv() * matV2;}// 如果发生退化，就对增量进行修改，退化方向不更新if (isDegenerate){cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}// 更新当前位姿 x = x + delta_xtransformTobeMapped[0] += matX.at<float>(0, 0);transformTobeMapped[1] += matX.at<float>(1, 0);transformTobeMapped[2] += matX.at<float>(2, 0);transformTobeMapped[3] += matX.at<float>(3, 0);transformTobeMapped[4] += matX.at<float>(4, 0);transformTobeMapped[5] += matX.at<float>(5, 0);float deltaR = sqrt(pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));// delta_x很小，认为收敛if (deltaR < 0.05 && deltaT < 0.05) {return true; }// 否则继续优化return false; }

其中一步对H矩阵做了特征值分解，若特征值太小，则说明优化方向的曲率过小，在SLAM中认为这种情况下为退化环境，非线性优化无法得到一个最优的结果。

例如长廊下的退化环境，进行scan-to-map的非线性优化时，此时求解H的特征值就会出现很小的情况(由于长廊中每个位置的差异性很小，雷达帧点云和长廊的局部地图的点云配准的残差都比较小，其非线性优化的曲率就会很小)，就容易无法得到一个很好的优化结果。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://xiahunao.cn/news/2815365.html

如若内容造成侵权/违法违规/事实不符，请联系瞎胡闹网进行投诉反馈，一经查实，立即删除！

相关文章

雷赛控制卡的扩展IO在雷赛软件里连不上是什么问题

雷赛控制卡的扩展IO在雷赛软件里连不上是什么问题

雷赛控制卡的扩展IO在雷赛软件里连不上是什么问题 解决: 要拨码的,拨了码&#xff0c;记得断电
阅读更多...
Redis之一： 简介及环境安装搭建

Redis之一： 简介及环境安装搭建

什么是NoSQL? NoSQL&#xff0c;指的是非关系型的数据库。NoSQL有时也称作Not Only SQL的缩写&#xff0c;是对不同于传统的关系型数据库的数据库管理系统的统称。 NoSQL用于超大规模数据的存储。&#xff08;例如谷歌或Facebook每天为他们的用户收集万亿比特的数据&#xf…
阅读更多...
Sentinel实战(待完善)

Sentinel实战(待完善)

目录 服务雪崩 什么是服务雪崩 服务不可用原因 解决方案 技术选型对比 Sentinel 介绍 优点 核心概念 资源 规则 代码实战 API实现 SentinelResource注解埋点实现 服务雪崩 什么是服务雪崩 在服务调用链路中, 服务提供者不可用, 导致服务调用者不可用, 间接让上上游…
阅读更多...
【Java EE初阶二十六】简单的表白墙（二）

【Java EE初阶二十六】简单的表白墙（二）

2. 后端服务器部分 2.1 服务器分析 2.2 代码编写 2.2.2 前端发起一个ajax请求 2.2.3 服务器读取上述请求,并计算出响应 服务器需要使用 jackson 读取到前端这里的数据,并且进行解析&#xff1a; 代码运行图&#xff1a; 2.2.4 回到前端代码&#xff0c;处理服务器返回的响应…
阅读更多...
【网站项目】424学报稿件管理系统

【网站项目】424学报稿件管理系统

&#x1f64a;作者简介&#xff1a;拥有多年开发工作经验&#xff0c;分享技术代码帮助学生学习&#xff0c;独立完成自己的项目或者毕业设计。 代码可以私聊博主获取。&#x1f339;赠送计算机毕业设计600个选题excel文件&#xff0c;帮助大学选题。赠送开题报告模板&#xff…
阅读更多...
【漏洞复现】大华智慧园区综合管理平台信息泄露漏洞

【漏洞复现】大华智慧园区综合管理平台信息泄露漏洞

Nx01 产品简介 大华智慧园区综合管理平台是一款综合管理平台&#xff0c;具备园区运营、资源调配和智能服务等功能。该平台旨在协助优化园区资源分配&#xff0c;满足多元化的管理需求&#xff0c;同时通过提供智能服务&#xff0c;增强使用体验。 Nx02 漏洞描述 大华智慧园区…
阅读更多...
Python多功能课堂点名器、抽签工具

Python多功能课堂点名器、抽签工具

一、问题缘起 去年&#xff0c;ChatGPT浪潮袭来&#xff0c;我懂简单的Python基础语法&#xff0c;又有一些点子&#xff0c;于是借助于人工智能问答工具&#xff0c;一步一步地制作了一个点名器&#xff0c;也可以用于抽签。当时&#xff0c;我已经设计好页面和基础的功能&am…
阅读更多...
通过QScrollArea寻找最后一个弹簧并且设置弹簧大小

通过QScrollArea寻找最后一个弹簧并且设置弹簧大小

项目原因&#xff0c;最近需要通过QScrollArea寻找其中最后一个弹簧并且设置大小和策略&#xff0c;因为无法直接调用UI指针&#xff0c;所以只能用代码寻找。 直接上代码&#xff1a; if (m_scrollArea){int iScrollWidth m_labelSelectedTitle->width();m_scrollArea-&g…
阅读更多...
vLLM: Easy, Fast, and Memory-Efficient LLM Serving with PagedAttention

vLLM: Easy, Fast, and Memory-Efficient LLM Serving with PagedAttention

论文&#xff1a;https://arxiv.org/abs/2309.06180 代码&#xff1a;GitHub - vllm-project/vllm: A high-throughput and memory-efficient inference and serving engine for LLMs 核心要点 为什么切换简单&#xff1f;Easy 本质是优化了底层推理过程的内存调用逻辑&#x…
阅读更多...
2月28日代码随想录二叉搜索树中的众数

2月28日代码随想录二叉搜索树中的众数

摸了一个寒假了&#xff0c;得赶一赶了 251.二叉搜索树中的众数 给你一个含重复值的二叉搜索树&#xff08;BST&#xff09;的根节点 root &#xff0c;找出并返回 BST 中的所有 众数&#xff08;即&#xff0c;出现频率最高的元素&#xff09;。 如果树中有不止一个众数&am…
阅读更多...
并查集例题（食物链）C++（Acwing）

并查集例题（食物链）C++（Acwing）

代码&#xff1a; #include <iostream>using namespace std;const int N 50010;int n, m; int p[N], d[N];int find(int x) {if(p[x] ! x){int t find(p[x]);d[x] d[p[x]];p[x] t;}return p[x]; }int main() {scanf("%d%d", &n, &m);for(int i 1…
阅读更多...
微服务Springcloud智慧工地APP源码 AI人工智能识别 支持多工地使用

微服务Springcloud智慧工地APP源码 AI人工智能识别 支持多工地使用

目录 一、现状描述 二、行业难点 APP端功能 一、项目人员 二、视频监控 三、危大工程 四、绿色施工 五、安全隐患 AI智能识别 环境监测 实名制管理 智慧监测 智慧工地全套解决方案 一、现状描述 建筑工程建设具有明显的生产规模大宗性与生产场所固定性的特点。建…
阅读更多...
Docker（运维工具）—— 学习笔记

Docker（运维工具）—— 学习笔记

快速构建、运行、管理应用的工具 一、安装docker 参考Install Docker Engine on Ubuntu | Docker Docs 二、快速入门 1、镜像和容器 docker镜像可以做到忽略操作系统的差异&#xff0c;跨平台运行&#xff0c;忽略安装的差异 当我们利用Docker安装应用时&#xff0c;Dock…
阅读更多...
智能风控体系之滚动率矩阵

智能风控体系之滚动率矩阵

汇总收集网上相关数据风控应用&#xff0c;多多交流。 在信贷风控的建模场景中&#xff0c;围绕样本数据的目标变量Y定义&#xff0c;是非常重要且特别有意思的处理过程&#xff0c;原因是根据差异化的业务场景与数据形态&#xff0c;标签Y的定义逻辑没有固定方法&#xff0c;只…
阅读更多...
Java8 Stream操作流10条常用方法

Java8 Stream操作流10条常用方法

1.基础数据 Data AllArgsConstructor NoArgsConstructor public class User {private String name;private Integer age;private String sex;private String city; //城市private Integer money; //业绩金额 } //准备数据List<User> users new ArrayList<>();use…
阅读更多...
nginx介绍及编译安装

nginx介绍及编译安装

nginx介绍 是一个流行的开源的高性能的HTTP和反向代理服务器&#xff0c;也可以用作邮件代理服务器。它以其高性能、稳定性、丰富的功能集和低资源消耗而闻名 nginx特点 高性能&#xff1a; Nginx以其高效的事件驱动架构而闻名&#xff0c;能够处理大量并发连接而不会消耗过多…
阅读更多...
阿里云服务器购买_价格_费用_云服务器ECS——阿里云

阿里云服务器购买_价格_费用_云服务器ECS——阿里云

2024年最新阿里云服务器租用费用优惠价格表&#xff0c;轻量2核2G3M带宽轻量服务器一年61元&#xff0c;折合5元1个月&#xff0c;新老用户同享99元一年服务器&#xff0c;2核4G5M服务器ECS优惠价199元一年&#xff0c;2核4G4M轻量服务器165元一年&#xff0c;2核4G服务器30元3…
阅读更多...
基于springboot实现线上阅读系统项目【项目源码+论文说明】计算机毕业设计

基于springboot实现线上阅读系统项目【项目源码+论文说明】计算机毕业设计

基于springboot实现线上阅读系统演示 摘要 随着社会发展速度的愈来愈快&#xff0c;以及社会压力变化的越来越快速&#xff0c;致使很多人采取各种不同的方法进行解压。大多数人的稀释压力的方法&#xff0c;是捧一本书籍&#xff0c;心情地让自己沉浸在情节里面&#xff0c;以…
阅读更多...
变分自编码器VAE

变分自编码器VAE

文章目录 一、机器学习分类二、AE与VAE 一、机器学习分类 机器学习分为&#xff1a;有监督学习、无监督学习、半监督学习、自监督学习、强化学习、迁移学习。 1.有监督学习&#xff1a; ①解释&#xff1a;算法从标注的训练数据中学习&#xff0c;其中每个样本都有相应的输出…
阅读更多...
git之多人协作

git之多人协作

一.多⼈协作⼀ 目标&#xff1a;在同一个分支上完成多人协作 任务&#xff1a;在linux和windows两个用户下分别在远程仓库&#xff08;非master分支&#xff09;中添加“linux submit”和“windows submit” 现在我们在远程仓库dev分支下filetxt文件情况&#xff1a; 我们先…
阅读更多...
最新文章

Copyright @ 2022~2023