分类: 数学基础

使用 SVD 方法求解 ICP 问题

本文是结合《Least-Squares Rigid Motion Using SVD》和《Least-Squares Fitting of Two 3-D Point Sets》两篇文章写的一个总结,里面有一些是自己的理解不一定正确。 1 问题定义 假设我们有两个点云集合 和 ,则我们定义的 ICP 问题是通过最小化点对之间距离获得相应的 Pose: 其中 代表每个点的权重。R 和 t 是我们所要求的旋转矩阵和平移向量。 2 计算平移 我们分两步求解旋转和平移,首先求解 t。固定...

欧拉积分、中点积分与龙格-库塔积分

在 SLAM 系统中经常用到各种不同的数值积分方法,工程上最常见的有三种:欧拉积分(Euler method)、中点积分(Midpoint method)和龙格-库塔法积分(Runge–Kutta methods)。他们的区别就是如何用数值方法模拟一个斜率。这里简单总结如下: 一、欧拉积分 设有如下微分方程: 并且 和 时刻的差为 ,则欧拉积分定义为: 也就是说用 t 时刻的斜率作为整个 时刻的导数。 二、中点积分 设有如下微分方程: 并且 和 时刻的差为 ,则显式中点积分定义为: 则隐式中点积分定义为: 也就是说用 时刻的斜率作为整个 时刻的导数。 欧拉积分与中点积分都是一阶近似并没有本质不同,二者只是一阶导数所取位置不同,他们的性能也有差别,如下图所示,作为一阶积分近似方法来讲,中点积分有时会稍好一些(带来更快的收敛速度)。图示为方程 ...