2020年05月_td092

原创 QuadricSLAM阅读笔记

QuadricSLAM是object SLAM的一种解决方案，可以提升定位的鲁棒性。物体的三维表示可以是CAD模型、点云模型、矩形包围盒、二次曲面等。QuadricSLAM使用的是二次曲面中的椭球面。椭球面的定义是：XTQX=0X^TQX=0XTQX=0其中XXX为面上的点，π=QX\pi = QXπ=QX为切平面。QQQ为4x4的对称矩阵，共10个自由度，包括一个尺度。这里使用的它的对偶:πTQ∗π=0\pi^TQ^*\pi = 0πTQ∗π=0非退化情况下，Q∗=Q−1Q^*=Q^{-1}

2020-05-29 15:49:23 697 2

原创 LSD直线检测算法

相比于点特征，直线段特征对视角和光照变化具有很好的鲁棒性。常用的直线检测有Hough直线检测和LSD等.Hough直线检测将直线检测映射为参数空间中点的检测，而LSD通过查找近似矩形区域来获得直线。相比于Hough直线检测，LSD算法具有结果准确，误检可控，同时不需要调节参数的优点。这里主要介绍LSD算法。下面是LSD算法流程：高斯降采样即高斯模糊后降采样，这里是为了减弱锯齿效应梯度计算这里梯度的大小定义为：G(x,y)=gx2(x,y)+gy2(x,y)G(x,y)=\sqrt{g_x^2(

2020-05-18 13:04:13 5014

原创利用舒尔补进行边缘化

边缘化是SLAM中常用的消元方法，能够有效减少计算量。在SLAM的BA过程中，对于状态变量的更新，可以给出一般化的更新方程：Hδx=gH\delta x = g Hδx=g其中，HHH是信息矩阵。上面的方程可以写成：[ΛaΛbΛbTΛc][δxaδxb]=[gagb]\begin{bmatrix} \Lambda_a & \Lambda_b\\ \Lambda_b^T& \Lambda_c \end{bmatrix}\begin{bmatrix} \delta x_a \\ \de

2020-05-13 21:46:30 1141

原创信息滤波器

信息滤波器(IF)是卡尔曼滤波器(KF)的对偶滤波器。卡尔曼滤波器维护的变量包括状态均值μ\muμ和状态协方差Σ\SigmaΣ。而信息矩阵维护的变量包括信息向量ξ\xiξ和信息矩阵Ω\OmegaΩ。从KF到IF在KF中我们用高斯分布来描述状态变量的分布情况:p(x)=det(2πΣ)−12exp(−12(x−μ)TΣ−1(x−μ))p(x) = det(2\pi\Sigma)^{-\frac{1}{2}}exp(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu))p(x)=

2020-05-12 18:15:21 1549

原创 IMU标定算法

IMU运动模型IMU由加速计和陀螺仪构成，理想情况下，加速度计和陀螺仪的3个轴都是正交的。但是对于一般的IMU，这是不现实的。那么，在加速度计和陀螺仪的测量值sSs^SsS与准确值SBS^BSB存在转换矩阵：Ta=[1−αyzαzy01−αzx001]T^a = \begin{bmatrix}1&-\alpha_{yz}&\alpha_{zy}\\0&1&...

2020-05-07 08:50:58 7607

原创粒子滤波器

粒子滤波器是一种常见的非线性滤波器，与卡尔曼类滤波器相比，它的适用性更广。粒子滤波器的主要步骤有：1、粒子集初始化2、更新粒子集权重3、重采样4、计算估计值和协方差矩阵1、粒子集初始化根据初始状态X0X_0X0的先验概率矩阵p(X0)p(X_0)p(X0)生成NNN个粒子χ0(i)\chi_0^{(i)}χ0(i)，每个粒子权重初始化为w0(χ0(i))=1Nw_0(\chi_...

2020-05-05 09:12:59 2146 1

原创 SURF算子

SURF可以看作是SIFT的改进型，它通过使用减少特征描述子维数，引入积分图的方式降低了计算量。提取SURF特征的一般步骤如下：构造Hessian矩阵，获取响应。构造高斯金字塔确定特征方向构造特征描述子1、Hessian矩阵就是指图像像素强度的二阶导。为了增强特征的鲁棒性，Hessian矩阵上还附加了高斯平滑：这里用可以用盒式滤波器来近似，这样在求解后面的Dxx（Lxx），Dy...

2020-05-03 18:12:24 596

原创 Harris角点检测

Harris角点是一种常见的局部特征子，可以用在相机标定等场景，比如我之前的博客相机标定的实现。下面介绍提取Harris特征点的一般步骤：图像平滑，这里一般是通过高斯滤波减少噪声的影响。计算图像的横向和纵向梯度。计算自相关矩阵计算角点能量，获取候选角点非极大值抑制1、计算图像梯度使用的是sobel或者prewitt算子。2、自相关矩阵的定义如下：它是2x2的矩阵，这里的w(...

2020-05-02 21:50:51 456

原创 numpy拼接数组

经常用到需要扩展的数组，一般用list不断地append。但是，这样有时候就免不了将list转化成ndarray的麻烦。查了一下，numpy也有数组拼接的函数，比如append、concatenate、vstack等。尝试了一下，append默认返回是一维数组，concatenate可以拼接多个多个数组，速度还据说更快，但也是会拉平维数，只有vstcak满足需要。示例：np.vstack((np.ones(3),np.ones(3)))array([[1,1,1],[1,1,1]])...

2020-05-28 21:01:18 298

原创 Eigen 的cwiseAbs2用法

cwiseAbs2()是求元素的平方。在numpy中对应np.power(m,2)

2020-05-28 16:40:04 3310

原创 numpy multi_dot的用法

multi_dot可以用于多个矩阵的乘法，而且会根据最少的计算量进行优化。矩阵相乘的计算量示例：A(10,100) B(100,5)C(5,50)cost((AB)C) = 101005 + 10550 = 5000 + 2500 = 7500cost(A(BC)) = 1010050 + 100550 = 50000 + 25000 = 75000用法：np.linalg.multi_dot([A, B, C, D])参考：https://numpy.org/doc/1.18/ref

2020-05-28 16:12:07 1099

原创 python Enum的使用

Enum是枚举类，用法如下：from enum import Enumclass Calendar: Month = Enum('Calendar.Month', ('Jan', 'Feb', 'Mar', 'Apr', 'May', 'Jun', 'Jul', 'Aug', 'Sep', 'Oct', 'Nov', 'Dec'))这里，Calendar.Month是枚举变量的类型名，Month为类名。调用方式为Month.Jan(默认为1），该变量类型为<Calendar.Month.J

2020-05-27 18:30:15 319

原创 opencv中检测到的角点与计算的描述子数量不一致

在opencv中获取图像特征子需要先通过FeatureDetector::detect检测角点，再通过DescriptorExtractor::compute计算描述子，但是返回的特征子数量一般要少于检测到的角点数。这是因为剔除了一些不适合生成描述子的角点，比如角点的方向不唯一。参考：https://docs.opencv.org/2.4/modules/features2d/doc/common_interfaces_of_descriptor_extractors.html...

2020-05-27 12:03:38 404

原创 python defaultdict用法

在使用dict()类型时，如果访问不存在的key时，会返回keyError。但有时候希望它能够返回一个默认值，这里就可以使用defaultdict。用法如下：from collections import defaultdictparams = defaultdict(bool)if not params['is_initialized']: params['is_initialized'] = True...

2020-05-26 09:34:17 423

原创粒子群优化

粒子群优化算法是一种简单、强大的启发式优化算法。它的一般步骤如下：在参数空间内，随机初始化粒子群，包括位置和速度。计算每个粒子对目标函数的适应值更新粒子个体的历史最优位置、局部最优位置、全局最优位置根据粒子与上述最优位置的差值更新粒子速度(类比梯度），并更新位置。终止条件是小于粒子群总误差小于阈值或迭代次数达到上限，否则回到第二步返回全局最有位置。参考实现：https://github.com/dockleryxk/PSOimport sys import mathimport r

2020-05-17 16:39:53 896

原创 LQR控制器

LQR控制器是常见的最优控制器，它的主要特点是将控制量加入到了成本函数中。对于线性系统：x˙=Ax+Bu\dot x = Ax+Bux˙=Ax+Bu设计控制器u=−Kxu = -Kxu=−Kx使得J=12∫0∞xTQx+uTRu dtJ = \frac{1}{2}\int_0^\infty x^TQx+u^TRu \,dtJ=21∫0∞xTQx+uTRudt最小。其中，xTQxx^TQxxTQx是状态部分，uTRuu^TRuuTRu是控制部分。为了求解K,需要引入Riccati微分方程：P

2020-05-15 17:08:13 3705

原创 RRT算法

快速搜索随机树(RRT)是一种基础的全局路径搜索算法，它具有简单、快速的特点。但是由于叶子是随机生成的，它获得的往往不是最优路径。RRT算法的流程如下：　　1. 将起点作为根节点；　　2. 在机器人的工作空间中，随机生成一个位姿点。　　3. 在位姿树上寻找2中位姿点的最近邻位姿点。　　4. 在3中的两个位姿构造插值点，判断它们是否与障碍物碰撞。如不碰撞，将2中的位姿加入位姿树。　　4. 判断随机位姿是否到达目标点附近或迭代步数到达最大值，如到达目标点，溯源获得路径。参考代码：https://g

2020-05-15 08:44:48 3884 4

原创 A* 算法

A*算法是一种常用的最短路径算法。它的算法流程如下：1、将地图栅格化，并初始化open_set和close_set。2、将起始点放入close_set,假如起始点的邻近点既没有在开放列表或封闭列表里面，计算该邻近点的代价函数F = G + H，并将其加入open_set，记录它的父节点。4、判断open_set是否为空，如果没有说明在达到结束点前已经找完了所有可能的路径点，寻路失败，算法结束；否则继续。5、访问open_set中代价最小的那个节点,将其从open_set移入close_set。6、

2020-05-14 15:52:12 1628

原创 np.atleast_2d用法

将输入的数组转化为至少两维：np.atleast_2d(3.0)array([[ 3.]])np.atleast_2d(1, [1, 2], [[1, 2]])[array([[1]]), array([[1, 2]]), array([[1, 2]])]参考：https://docs.scipy.org/doc/numpy-1.16.0/reference/generated/numpy.atleast_2d.html

2020-05-12 07:05:18 11925

原创 VMware修改ubuntu虚拟机磁盘大小

之前装ubuntu只给了40G的磁盘大小，结果装个ROS七七八八的就满了，然后就想扩容一下。步骤入下：1、在虚拟机设置中将磁盘扩展2、打开对应的虚拟机Disks应用，为新增的空间分区...

2020-05-04 11:37:23 1615

realjc的博客