- 博客(232)
- 资源 (2)
- 收藏
- 关注
RSS订阅
原创 icp算法原理与实现
ICP算法原理ICP是一种经典的点云匹配算法，用于估计两个点云P、M之间的位姿变换。ICP的一般流程：1、获得初始位姿(初始位姿对最终的匹配结果有较大影响)2、迭代运行如下步骤：为点云P中的点寻找M中的匹配点依据匹配，计算P、M之间的位姿上面的步骤中，如何寻找匹配点和计算位姿的损失是关键。ICP算法C++实现point2point 解析法...
2021-02-28 22:27:35
1567
原创 SVO中的map类
一个完整的SLAM中需要有一个map类来维护关键帧、地图点，重定位、回环乃至之后的路径规划、环境交互都依赖于此。这里构建的一个简单的map类，代码未经过测试， 仅作参考。(待续）这里主要考虑Map、Frame、Feature、Point四个类。class Frame;class Point{public: cv::Point2f* xy_; vector<Frame*> obs_; void deleteFrameRef(Frame* frame);};c
2020-10-23 22:22:10
153
原创 SVO的初始化
利用LK光流和本质矩阵恢复的初始化：待更#include <opencv2/features2d.hpp>#include <opencv2/core/types.hpp>#include <opencv2/calib3d.hpp>#include <opencv2/video/tracking.hpp>#include<iostream>#include<vector>using namespace std;cl
2020-10-22 23:53:24
322
原创 LK光流法
光流是用来描述特征点在图像上的运动的属性。特征点的灰度值可以表示为：这里，一般假设运动过程中灰度不变，所以：这里的VxV_xVx、VyV_yVy被称为光流。
2020-10-15 21:10:34
2118
原创 MSCKF中的IMU积分
这里，采用了4阶龙格库塔方法对两个IMU帧之间的位置和速度进行积分。首先，考虑IMU的位置和速度微分公式：p˙(t)=v(t)v˙(t)=q(t)a+g\dot{p}(t) = v(t) \\ \dot{v}(t)=q(t)a+gp˙(t)=v(t)v˙(t)=q(t)a+g这里的q(t)是IMU的位姿q(t)是IMU的位姿q(t)是IMU的位姿。龙格库塔方法可以简单理解为，通过多个斜率逼近微分方程的导数。4阶龙格库塔方法可以表示为：这里的q(t+Δt)q(t+\Delta t)q(t+Δt
2020-10-14 13:43:24
657
原创 SVIn2阅读笔记
SVIn2是一种针对水下环境的SLAM系统，该框架包括了声纳、相机、里程计、深度计传感器。这里针对的测绘场景是水下洞穴。系统的结构框图：损失函数定义为：其中，ere_rer为反投影误差、ese_ses为IMU误差、ete_tet为声纳误差、eue_ueu为深度计误差。声纳误差可以表示为：水下系统的成功对初始化的要求很高。深度传感器与相机配合来确定尺度：IMU与相机之间的约束同样用来确定尺度：预积分公式：IMU与相机的损失函数：加上了回环的损失函数：实验
2020-08-24 11:54:59
571
原创 Vid2Curve阅读笔记
Vid2Cur是一种基于曲线的SfM方法，针对结构较细的物体效果很好。下面介绍它的算法流程。1、预处理这里我们假设图片拍摄的背景是干净的，可以很方便的将待重建物体分割出来。然后，利用图像细化算法获得一个像素宽的骨架曲线。2、相机位姿估计首先定义3D曲线为G=(P,E)G=(P,E)G=(P,E),PPP为三维点，EEE为三维点的连通性。接着定义在相机位姿(Rk,Tk)(R_k,T_k)(Rk,Tk)下的损失函数：这里，两个2D曲线之间的距离定义为;即最近点之间距离的和。2.1、初始.
2020-08-21 18:19:13
1925
6
原创 移动机器人平台常用传感器简介
在移动机器人上常用的传感器包括激光雷达、毫米波雷达、相机、IMU、编码器等，其中激光雷达、超声波雷达和相机是用来测量外部环境的，IMU、编码器测量的是AGV自身位姿。下面分别描述它们的原理、适用场景及缺陷。激光雷达按照机械结构可以分为机械雷达和固态雷达。机械激光雷达通过机械旋转机构调整激光发射角度，产品较为成熟。固态雷达可分为OPA、MEMS、Flash等类型，内部没有旋转部件，体积较机械雷达小。激光雷达的优势是定位精度高、探测距离远，劣势是成本高，分辨率较低。使用激光雷达进行导航的研究起步早，是当前最
2020-08-14 18:21:07
2954
原创 在gazebo中添加Aruco码
gazebo中一个model文件的内容如下：model_name model.sdf .sdf文件 model.config .config文件 materials 文件夹 | --scripts 文件夹 model.material 纹理信息 | --textures 文件夹 model.png 图像下面的代码可以自动产生对应的Aruco Marker模型import cv2 import cv2.aruco as aru
2020-08-07 19:57:20
1098
原创 VINS-mono阅读笔记
VINS-mono是当前常用的VIO框架之一，出自港科大的沈劭劼老师团队。VINS-mono是基于优化的紧耦合方案，精度、速度、鲁棒性俱佳。整体框架从上图可得，VINS-mono可以分为4个部分，跟踪、初始化、滑窗优化、回环优化。跟踪视觉跟踪这里采用的是光流跟踪的前端，一般过程是通过特征点匹配获得本质矩阵或者基础矩阵，然后恢复出R，t;之后再将特征点三角化，后面就可以通过pnp求解位姿。对于关键帧的选择有以下两个准则：1、当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧（因为视差可
2020-06-29 23:24:52
298
原创 对极约束及极线搜索
对极约束是指参考帧与当前帧光心与观测点组成的平面上的几何约束。基于运动关系和共面约束，可以推出本质矩阵：对于x′x'x′,可以看作是直线ax′+by′+c′=0ax'+by'+c'=0ax′+by′+c′=0上的点，直线参数由下面公式给出这样将x′x'x′代入直线方程，得到的结果即为对极误差。...
2020-06-20 11:21:59
2008
原创 三角测量原理
如上图所示，三角测量是指在已知当前帧和参考帧的位姿和匹配点以及相机内参的俄情况下，恢复观测点深度的过程。根据当前帧和参考帧的投影方程有：将上面的方程合并，并展开：通过SVD分解就可以求解上式。参考：https://www.uio.no/studier/emner/matnat/its/nedlagte-emner/UNIK4690/v16/forelesninger/lecture_7_2-triangulation.pdf...
2020-06-20 08:31:38
4399
原创 An Efficient Solution to the Five-Point Relative Pose Problem阅读笔记
本文介绍的是通过五点法恢复运动。求解本质矩阵首先根据对极约束有:q′TEq=0q'^TEq=0q′TEq=0这个矩阵将E展开，可得：q^TE^=0\hat{q}^T\hat{E} = 0q^TE^=0其中，这里的本质矩阵E有9个自由度（实际上5个，R,t减去尺度），5个点只能提供5个约束，所以E可以表示为X,Y,Z,WX,Y,Z,WX,Y,Z,W的线性组合：这里不妨令w=1w = 1w=1,只剩下x,y,zx,y,zx,y,z三个未知数。又考虑到本质矩阵的性质：将上式代入其中，可以获
2020-06-17 22:35:16
833
原创 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
662
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
4841
原创 利用舒尔补进行边缘化
边缘化是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
1063
原创 信息滤波器
信息滤波器(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
1436
原创 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
7464
原创 粒子滤波器
粒子滤波器是一种常见的非线性滤波器，与卡尔曼类滤波器相比，它的适用性更广。粒子滤波器的主要步骤有：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
1973
1
原创 SURF算子
SURF可以看作是SIFT的改进型，它通过使用减少特征描述子维数，引入积分图的方式降低了计算量。提取SURF特征的一般步骤如下：构造Hessian矩阵，获取响应。构造高斯金字塔确定特征方向构造特征描述子1、Hessian矩阵就是指图像像素强度的二阶导。为了增强特征的鲁棒性，Hessian矩阵上还附加了高斯平滑：这里用可以用盒式滤波器来近似，这样在求解后面的Dxx（Lxx），Dy...
2020-05-03 18:12:24
566
原创 Harris角点检测
Harris角点是一种常见的局部特征子，可以用在相机标定等场景，比如我之前的博客相机标定的实现。下面介绍提取Harris特征点的一般步骤：图像平滑，这里一般是通过高斯滤波减少噪声的影响。计算图像的横向和纵向梯度。计算自相关矩阵计算角点能量，获取候选角点非极大值抑制1、计算图像梯度使用的是sobel或者prewitt算子。2、自相关矩阵的定义如下：它是2x2的矩阵，这里的w(...
2020-05-02 21:50:51
430
原创 李代数在BA中的应用
首先，给出李代数(so3,se3so3,se3so3,se3)与对应李群(SO3,SE3SO3,SE3SO3,SE3)的定义如下：参考我之前的博客位姿表示方法，so3so3so3其实就是罗德里格斯表示法。李代数的作用就是建立起了旋转矩阵和旋转向量之间的指数(对数)关系，这有利于在优化过程中的求导运算。下面参考https://github.com/izhengfan/ba_demo_cere...
2020-04-26 10:47:29
552
原创 方差的无偏估计的证明
无偏估计的定义如下：对随机变量XXX的估计Xˉ\bar{X}Xˉ,如果E[Xˉ]=E[X]E[\bar{X}] = E[X]E[Xˉ]=E[X],则称Xˉ\bar{X}Xˉ是XXX的无偏估计。根据方差的定义展开得：E(∑i=1n(Xi−Xˉ)2)=∑i=1nE(Xi2)−nE(Xˉ2)E(\sum_{i=1}^n(X_i-\bar{X})^2)= \sum_{i=1}^nE(X_i^2)-n...
2020-04-22 09:30:42
15242
原创 IMU预积分
在VO算法中，可以通过$PNP$获取两个关键帧之间的位姿，并通过BA优化反投影误差。而VIO算法中通过IMU预积分同样可以获得关键帧之间的位姿，为BA增加约束。
2020-04-21 14:47:08
761
原创 UKF原理与实现
卡尔曼滤波家族主要有KF、EKF、UKF三兄弟。KF主要应用于系统方程和量测方程为线性的的场景，EKF应用于轻度非线性场景，而UKF则在强非线性情况下有更好的表现。这里只介绍UKF。...
2020-04-21 10:34:40
11476
5
原创 矩阵特征值的几何意义
对于一个二阶矩阵AAA，它的特征值为λ1\lambda_1λ1和λ2\lambda_2λ2,对应的特征向量为α1\alpha_1α1和α2\alpha_2α2。考虑线性变换，[x1x2]=A[u1u2]\begin{bmatrix}x_1\\x_2\end{bmatrix}=A\begin{bmatrix}u_1\\u_2\end{bmatrix}[x1x2]=A[u1u2],...
2020-04-21 07:31:35
1758
原创 里程计运动模型
这里给出两轮式带里程计小车的运动模型。状态变量为(x,y,θ)(x,y,\theta)(x,y,θ),分别是位置和方向角。位移增量为Δs\Delta sΔs,角度增量为Δθ\Delta \thetaΔθ。则运动方程：[xyθ]t=[xyθ]t−1+[Δscos(θ+Δθ/2)Δssin(θ+Δθ/2)Δθ]\begin{bmatrix} x\\y\\\theta\end{bmatrix}...
2020-04-08 09:00:01
779
原创 python调用c++接口(pybind11)
pybind11是一个将c++接口转接给python的库，它支持C++ 11标准的编译器。这里我做了一个简单的实验，主要是验证将Eigen::MatrixXf类型映射到numpy::ndarray类型,这样就可以在python愉快地调用C++函数了。完整代码见：首先，python脚本代码：#!/usr/bin/env python# -*- coding: utf-8 -*-impo...
2020-04-04 09:06:32
1845
原创 词嵌入矩阵
在nlp任务中，计算机不会直接处理文本信息，而是需要先将文本转化为矩阵进行处理。通常，先将句子划分为单词，每个单词用一个固定维数的向量表示（比如300），而句子的维数也会固定为一个常值(max_len)，并进行补齐。词嵌入矩阵就是将单词从one-shot形式转化为固定维数的向量时所需的转换矩阵。英文单词的oneshot形式可由tokenizer.word_index直接获得，它是一个字典，内...
2020-04-01 21:23:27
2851
原创 Hand-eye Calibration: 4D Procrustes Analysis Approach阅读笔记
首先看下实验部分：这篇论文提出的手眼标定算法在精度和速度上表现得都很好。下面介绍算法流程：1、通过T=[Rt01]↔RT,SO(4)=[Rϵt−ϵtTR1]T=\begin{bmatrix}R&t\\0&1\end{bmatrix}\leftrightarrow R_{T,SO(4)}=\begin{bmatrix}R&\epsilon t\\-\epsilon t^...
2020-03-26 00:27:42
290
原创 对偶四元数的应用
对偶四元数的定义对偶四元数的一般形式为：q^=r+εs\hat{q}=r+\varepsilon sq^=r+εs,其中r=r0+ir1+jr2+kr3r=r_0+ir_1+jr_2+kr_3r=r0+ir1+jr2+kr3、s=s0+is1+js2+ks3s=s_0+is_1+js_2+ks_3s=s0+is1+js2+ks3均为四元数，ε\varepsilonε为对偶运算符...
2020-03-25 14:50:09
2448
2
原创 Frank C. Park手眼标定方法
Frank C. Park与Tsai都是采用了两步法，但它利用李代数求解AX=XB方法。由logA=logXBXTlog A = log XBX^TlogA=logXBXT有[a]=X[b]xT[a]=X[b]x^T[a]=X[b]xT,[][][]是so3形式的旋转向量所以X=(MTM)−12MTX=(M^TM)^{-\frac{1}{2}}M^TX=(MTM)−21MT,M=∑i=1k...
2020-03-22 00:00:35
605
原创 Tsai算法
Tsai算法主要用于求解手眼标定中的AX=XB问题，它属于二阶段法，先求解旋转R后求平移t。首先，求解R:skew(Pgij+Pcij)Pcg′=Pcij−Pgijskew(P_{gij}+P_{cij})P'_{cg}=P_{cij}-P_{gij}skew(Pgij+Pcij)Pcg′=Pcij−Pgij其中：P=2sinθ2uTP=2sin\frac{\theta}{2}...
2020-03-19 10:20:43
1979
4
原创 手眼标定eye-in-hand、eye-to-hand
手眼标定的目标是估计相机与机器人之间的转换关系，可分为eye-in-hand、eye-to-hand两种情况。这两种情况都可以统一到AX=XBAX=XBAX=XB的求解框架下，下面推导这一过程：eye-on-handTgrip1cam1Tcam1base1Tbase1end1=Tgrip2cam2Tcam2base2Tbase2end2T^{cam1}_{grip1}T^{base1}...
2020-03-18 23:17:45
3464
原创 最小二乘估计
1、最小二乘估计最小二乘估计最早是由高斯在1795年提出。设状态变量为XXX,观测变量为ZZZ，观测噪声为VVV，则第iii次观测可以表示为：Zi=HiX+ViZ_i=H_iX+V_iZi=HiX+Vi最小二乘估计的目标是找到X=X^X=\hat{X}X=X^使得：J(X)=(Z−HX)T(Z−HX)J(X) = (Z-HX)^T(Z-HX)J(X)=(Z−HX)T(Z−HX)最小...
2020-02-26 10:09:11
6667
1
原创 相机标定的实现
获取棋盘格角点board, corners = cv2.findChessboardCorners(gray, (CHESSBOARD_CORNERS_ROWCOUNT,CHESSBOARD_CORNERS_COLCOUNT), None)亚像素细化corners_acc = cv2.cornerSubPix( image=gray, corner...
2019-12-31 00:54:08
303
1
原创 卡尔曼滤波公式及推导
卡尔曼滤波1、简介\qquad卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。2、离散卡尔曼滤波器状态方程：xk=Axk−1+Buk−1+wk−1x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1}xk=Axk−1+Buk−1+wk−1观测方程：zk=Hxk+vkz_k = Hx_k+...
2019-06-10 23:27:54
868
原创 位姿表示方法
5、四元数的乘法def multiply_quaternions(q, r): """ Takes in 2 1*4 quaternions, returns a 1*4 quaternion""" """Multiplies two quaternions 'q' and 'r'. Returns 't' where t = q*r""" t = np.emp...
2019-05-27 09:51:03
7752
2
camera-calibration-python.zip
2019-12-30
空空如也
TA创建的收藏夹 TA关注的收藏夹
TA关注的人