![](https://img-blog.csdnimg.cn/20201014180756930.png?x-oss-process=image/resize,m_fixed,h_64,w_64)
机器人
td092
役に立つことをする
展开
-
SLAM结果的评价
目录时间戳对齐时间戳处理位姿对齐计算ATE、RTE时间戳对齐SLAM结果的评价主要是指计算估计的轨迹与Groundtruth的ATE、RTE等指标。由于一般情况下,估计的轨迹不会包括所有时间戳,需要找到与它最近的一帧真值作为对应帧。一种简单的思路就是对所有帧的时间戳作差并从小到大排序,依次获得对应帧。def associate(first_list, second_list,offset,max_difference): """ Associate two dictionaries原创 2021-08-15 23:32:26 · 484 阅读 · 0 评论 -
将点云转化为voxel
将点云转化为voxel一般流程:1、确定点云的范围2、划分网格3、将点云投影到网格一个例子:https://github.com/hardyqr/PointcloudVoxelizer def __get_3D_matrix__(self, full_list, list_matrix, xyz_range, mag_coeff,_format): lx,ly,lz,hx,hy,hz = xyz_range magnifier = int(mag_coeff原创 2021-05-25 09:46:46 · 1578 阅读 · 0 评论 -
KITTI 3D数据集获取右目的2D检测框
KITTI数据集只提供了左目的2Dbbox,而如果要使用双目进行3D box估计,就需要先将3D box投影到右目获得真值。以下代码由Stereo RCNN 修改而来:#!/bin/usr/env python3import numpy as np import csv import math as mimport os def NormalizeVector(P): return np.append(P, [1])def E2R(Ry, Rx, Rz): '''Combi原创 2021-05-18 14:17:32 · 630 阅读 · 0 评论 -
利用李代数做位姿图优化的简单实现
#include<bits/stdc++.h>#include <ceres/autodiff_cost_function.h>#include <ceres/cost_function.h>#include <ceres/local_parameterization.h>#include <ceres/loss_function.h>#include <ceres/problem.h>#include <ceres.原创 2021-03-29 13:18:09 · 497 阅读 · 0 评论 -
ros subscriber callback 传递多个参数
添加callback_args参数,比如:rospy.Subscriber(self.ROV_name+topicx, msg_type, callback, callback_args=topicx, queue_size=queue_size)原创 2021-03-11 18:12:04 · 2844 阅读 · 0 评论 -
icp算法原理与实现
ICP算法原理ICP是一种经典的点云匹配算法,用于估计两个点云P、M之间的位姿变换。ICP的一般流程:1、获得初始位姿(初始位姿对最终的匹配结果有较大影响)2、迭代运行如下步骤:为点云P中的点寻找M中的匹配点依据匹配,计算P、M之间的位姿上面的步骤中,如何寻找匹配点和计算位姿的损失是关键。ICP算法C++实现point2point 解析法...原创 2021-02-28 22:27:35 · 1610 阅读 · 0 评论 -
IMU标定中的Allen方差
Allen方差Allen方差是IMU标定任务中的常用工具。IMU标定可以分为确定性误差和随机误差,确定性误差包括:scale、bias、misalignment。随机误差则主要是高斯白噪声和bias随机游走。Allen方差主要用于标定随机误差。Allen方差获取Allen方差的计算方法如下:保持IMU静止, 采集N个数据点。将N个点定长分组,常用的交叠式Allen方差如下(n=3):对于每个子集,计算均值后,计算相邻两组的方差,得到的时间-方差曲线即为Allen方差。Alle原创 2020-12-22 10:48:24 · 4694 阅读 · 0 评论 -
pcl为点云绘制包围盒
{ Eigen::Vector4f min, max; pcl::getMinMax3D(*vptr_clusters[i], min, max); visualization_msgs::Marker marker; marker.header.stamp = ros::Time::now(); marker.header.frame_id = frame_id_; marker.ns = "cluster_node"; marker.id = i; marker.type = vi.原创 2020-12-02 09:30:27 · 1485 阅读 · 4 评论 -
pcl 显示点云的几种方式
首先, 读取点云 pcl::PCDReader reader; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); reader.read (strPCDFilePath , *cloud);设置显示颜色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> yell原创 2020-11-30 18:09:15 · 5617 阅读 · 1 评论 -
ORB SLAM中的BA
这里是截取的ORB SLAM中的BA部分,是基于g2o实现的。代码未经过测试,仅作参考。待更。void Optimization::bundleAdjustment(vector<KeyFrame*> vpKFs, vector<MapPoints*> vpMPs, int nIterations,const unsigned int fixedID){ g2o::SparseOptimizer optimizer; g2o::BlockSolver_6原创 2020-11-01 21:38:03 · 838 阅读 · 0 评论 -
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 · 159 阅读 · 0 评论 -
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 · 329 阅读 · 0 评论 -
opencv Fast特征提取函数
在opencv中,Fast特征提取器的函数原型:static Ptr<FastFeatureDetector> cv::FastFeatureDetector::create ( int threshold = 10, bool nonmaxSuppression = true, FastFeatureDetector::DetectorType type = FastFeatureDetector::TYPE_9_16 )threshold指的是中心像素与周围像素强度的原创 2020-10-22 00:28:03 · 1461 阅读 · 0 评论 -
LK光流法
光流是用来描述特征点在图像上的运动的属性。特征点的灰度值可以表示为:这里,一般假设运动过程中灰度不变,所以:这里的VxV_xVx、VyV_yVy被称为光流。原创 2020-10-15 21:10:34 · 2144 阅读 · 0 评论 -
opencv undistortPoints用法
opencv中使用undistortPoints函数校正特征点的变形,对于针孔相机,可以使用cv::undistortPoints,而对于鱼眼相机则需要使用cv::fisheye::undistortPoints。函数原型:void cv::fisheye::undistortPoints ( InputArray distorted,OutputArray undistorted,InputArray K,InputArray D,InputArray R = noArray(),原创 2020-10-15 14:53:50 · 5441 阅读 · 0 评论 -
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 · 687 阅读 · 0 评论 -
MSCKF中的IMU参数标定
在MSCKF中,IMU的参数可以在线标定。在初始阶段,采集IMU不动时的200帧信号,平均得到的角速度为IMU角速度的偏置,平均线速度的大小为重力的大小,并求出平均线速度与世界坐标系的旋转。代码如下:void MsckfVio::initializeGravityAndBias() { // Initialize gravity and gyro bias. Vector3d sum_angular_vel = Vector3d::Zero(); Vector3d sum_linear_a原创 2020-10-13 22:37:50 · 431 阅读 · 0 评论 -
iMaxB6充电介绍
iMaxB6是一款多用途充电器,能够为Li-ion、Li-Poly、Li-Fe、Ni-Cd、Ni-MH和Pb类型电池充电,支持6串以内的平衡充电。简要步骤:1、连接正负电源;2、连接平衡线;3、设置充电电流、电压、充电模式4、常按start检测,短按start确定电压中2s、3s、4s指的是串联的电池节数,它们的标称电压和充电压分别是(7.2v 8.4v)、(10.8v 12.6v)、(14.4v 16.8v)平衡线是为了使串联的各节锂电池达到彼此之间相对平衡,使锂电池的寿命增长充电模式Bl原创 2020-08-28 18:37:48 · 1468 阅读 · 0 评论 -
SVIn2阅读笔记
SVIn2是一种针对水下环境的SLAM系统,该框架包括了声纳、相机、里程计、深度计传感器。这里针对的测绘场景是水下洞穴。系统的结构框图:损失函数定义为:其中,ere_rer为反投影误差、ese_ses为IMU误差、ete_tet为声纳误差、eue_ueu为深度计误差。声纳误差可以表示为:水下系统的成功对初始化的要求很高。深度传感器与相机配合来确定尺度:IMU与相机之间的约束同样用来确定尺度:预积分公式:IMU与相机的损失函数:加上了回环的损失函数:实验原创 2020-08-24 11:54:59 · 634 阅读 · 0 评论 -
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 · 1935 阅读 · 6 评论 -
VNC连接树莓派
首先在树莓派上安装vnc server:sudo apt install vnc4server -yvncserver##运行通过ifconfig命令查看局域网ip,一般是192.168.xxx.xxx然后,在客户机安装vncviewer,在地址栏输入上面的ip和运行vncserver后显示的端口号(第一次运行为1),即192.168.xxx.xxx:1最后,输入运行vncserver时设置的密码,即可见到树莓派桌面。...原创 2020-08-19 11:36:03 · 237 阅读 · 0 评论 -
移动机器人平台常用传感器简介
在移动机器人上常用的传感器包括激光雷达、毫米波雷达、相机、IMU、编码器等,其中激光雷达、超声波雷达和相机是用来测量外部环境的,IMU、编码器测量的是AGV自身位姿。下面分别描述它们的原理、适用场景及缺陷。激光雷达按照机械结构可以分为机械雷达和固态雷达。机械激光雷达通过机械旋转机构调整激光发射角度,产品较为成熟。固态雷达可分为OPA、MEMS、Flash等类型,内部没有旋转部件,体积较机械雷达小。激光雷达的优势是定位精度高、探测距离远,劣势是成本高,分辨率较低。使用激光雷达进行导航的研究起步早,是当前最原创 2020-08-14 18:21:07 · 3096 阅读 · 0 评论 -
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 · 4964 阅读 · 0 评论 -
粒子群优化
粒子群优化算法是一种简单、强大的启发式优化算法。它的一般步骤如下:在参数空间内,随机初始化粒子群,包括位置和速度。计算每个粒子对目标函数的适应值更新粒子个体的历史最优位置、局部最优位置、全局最优位置根据粒子与上述最优位置的差值更新粒子速度(类比梯度),并更新位置。终止条件是小于粒子群总误差小于阈值或迭代次数达到上限,否则回到第二步返回全局最有位置。参考实现:https://github.com/dockleryxk/PSOimport sys import mathimport r原创 2020-05-17 16:39:53 · 861 阅读 · 0 评论 -
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 · 3689 阅读 · 0 评论 -
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 · 1567 阅读 · 0 评论 -
信息滤波器
信息滤波器(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 · 1495 阅读 · 0 评论 -
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 · 7550 阅读 · 0 评论 -
粒子滤波器
粒子滤波器是一种常见的非线性滤波器,与卡尔曼类滤波器相比,它的适用性更广。粒子滤波器的主要步骤有: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 · 2090 阅读 · 1 评论 -
SURF算子
SURF可以看作是SIFT的改进型,它通过使用减少特征描述子维数,引入积分图的方式降低了计算量。提取SURF特征的一般步骤如下:构造Hessian矩阵,获取响应。构造高斯金字塔确定特征方向构造特征描述子1、Hessian矩阵就是指图像像素强度的二阶导。为了增强特征的鲁棒性,Hessian矩阵上还附加了高斯平滑:这里用可以用盒式滤波器来近似,这样在求解后面的Dxx(Lxx),Dy...原创 2020-05-03 18:12:24 · 581 阅读 · 0 评论 -
Harris角点检测
Harris角点是一种常见的局部特征子,可以用在相机标定等场景,比如我之前的博客相机标定的实现。下面介绍提取Harris特征点的一般步骤:图像平滑,这里一般是通过高斯滤波减少噪声的影响。计算图像的横向和纵向梯度。计算自相关矩阵计算角点能量,获取候选角点非极大值抑制1、计算图像梯度使用的是sobel或者prewitt算子。2、自相关矩阵的定义如下:它是2x2的矩阵,这里的w(...原创 2020-05-02 21:50:51 · 443 阅读 · 0 评论 -
UKF原理与实现
卡尔曼滤波家族主要有KF、EKF、UKF三兄弟。KF主要应用于系统方程和量测方程为线性的的场景,EKF应用于轻度非线性场景,而UKF则在强非线性情况下有更好的表现。这里只介绍UKF。...原创 2020-04-21 10:34:40 · 11619 阅读 · 5 评论 -
PnP问题求解
PnP算法是指已知3D点及其在相机上的投影,求相机姿态的问题。投影方程可以表示为:λ[uv1]=K[Rt][xyz1] \lambda \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} =K \begin{bmatrix} R & t \end{bmatrix}\begin{bmatrix} x \\ y \\ z\\1 \end{bmatrix...原创 2020-03-04 01:01:59 · 6895 阅读 · 0 评论 -
最小二乘估计
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 · 6836 阅读 · 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 · 312 阅读 · 1 评论 -
维纳滤波原理及实现
维纳滤波问题描述:设有一个线性系统,它的单位脉冲响应是h(n)h(n)h(n),当输入一个观测到的随机信号x(n)x(n)x(n),简称观测值,且该信号包含噪声w(n)w(n)w(n)和有用信号s(n)s(n)s(n),也即x(n)=w(n)+s(n)x(n)=w(n)+s(n)x(n)=w(n)+s(n)则输出(估计值)为:y(n)=x(n)∗h(n)=Σm=−∞+∞h(m)x(n−m...原创 2019-12-30 21:25:43 · 4999 阅读 · 0 评论 -
卡尔曼滤波公式及推导
卡尔曼滤波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 · 973 阅读 · 0 评论 -
相机参数DKRP的解释
在gazebo中添加kinect相机时,cameraInfo包含D、K、R、P四个矩阵。矩阵D是失真系数,包括(k1, k2, t1, t2, k3)矩阵K是相机内参,即[fx0cx0fycy001]\begin{bmatrix} fx&0&cx\\0&fy&cy\\0&0&1\end{bmatrix}⎣⎡fx000fy0cxcy1⎦⎤矩阵R是一个3✖3的旋转矩阵,仅对双目相机有效,使左右极线平行。矩阵P是投影矩阵,即[fx0cxTx0fyc原创 2020-08-11 20:12:43 · 7329 阅读 · 6 评论 -
gazebo如何删除添加的model path
在~/.gazebo/gui.ini文件中删除后,重新打开gazebo即可。原创 2020-08-08 12:11:35 · 982 阅读 · 0 评论 -
在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 · 1154 阅读 · 0 评论