- 博客(21)
- 收藏
- 关注
原创 Simulink用计数器降采样
需要将一些高频信号转成低频信号,但是不想降低数据发送的频率,用ZOH会降低数据频率,所以自己搭了一个计数器。fcn中的程序就是求余数的程序:function y = fcn(u)y = rem(u,50);结果:...
2021-08-01 17:41:42
1660
1
原创 RANSAC算法PCL实例
算法简介附上一个上古时期的博客:https://www.cnblogs.com/xrwang/archive/2011/03/09/ransac-1.htmlRANSAC从样本中随机选出一个样本的子集,使用最小方差估计算法对这个子集计算模型参数,然后计算所有样本与该模型的偏差,再用一个预先设定好的阈值与偏差比较,当偏差小于阈值时,该样本点属于模型内样本点(inliers),否则为外样本点(outliers),记录下当前inliers的个数,然后重复这个过程。每重复一次,都记录当前最佳的模型参数,所谓最佳
2020-12-27 16:36:50
1023
原创 VINS-mono代码阅读 -- 后端优化
书接上回,初始化完成以后,就是进行非线性优化了,我们来看看这个函数吧。1. 非线性优化1) 状态向量状态向量包括滑窗内的n+1个所有相机的状态,包括位置,朝向,速度,加速度计bias和陀螺仪bias,相机到IMU的外参,m+1个3D点的逆深度:X=[x0,x1,⋯ ,xcb,λ0,λ1,⋯ ,λm]X = [x_0,x_1,\cdots,x_c^b,\lambda_0,\lambda_1,\cdots,\lambda_m]X=[x0,x1,⋯,xcb,λ0,λ1,⋯,λm]xk=[pb
2020-09-05 12:43:46
458
原创 VINS-mono代码阅读 -- 初始化(三)
这一部分来看看视觉-IMU对齐。1. 视觉惯导联合初始化 VisualIMUAlignment这个函数调用了两个函数,1)陀螺仪bias校正 2)初始化速度、重力和尺度因子bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x){ //陀螺仪偏差标定 solveGyroscopeBias(all_im
2020-08-28 20:17:40
438
原创 VINS-mono代码阅读 -- 初始化
1. 初始化的流程初始化采用的是视觉和IMU松耦合的方案首先用SFM求解滑动窗口内所有帧的位姿,和所有路标点的3D位置,然后跟IMU预积分的值对齐,求解重力向量,尺度因子,陀螺仪bias及每一帧对应的速度。画一下初始化的流程图:在看程序之前,先来看看这里用到的数据结构: ImageFrame imageframe(image, header.stamp.toSec()); //tmp_pre_integration 是在processIMU() 时进行的 IntegrationBase
2020-08-19 10:18:55
546
原创 VINS-mono代码阅读 -- processImage
1. 对特征点信息的封装relocalization frame这部分的处理,我准备放到后面再写,现在就先看看ProcessImage这部分代码。在进行图像处理之前,先对接收到的特征点信息进行封装,image是建立每个特征点和(camera_id,[x,y,z,u,v,vx,vy])构成的map,索引为feature_id。然后调用processImage。map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>&g
2020-08-17 15:49:24
778
原创 VINS-mono代码阅读 -- IMU预积分
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity){ //1. 判断是不是第一个imu消息,如果是第一个imu消息,就将当前的加速度和角速度作为初始加速度和角速度 if (!first_imu)//first_imu 标志位,初始值为false { first_imu = true;
2020-08-16 14:52:53
440
原创 VINS-mono代码阅读 --process线程
process线程对接收到的IMU数据和特征点进行处理,并实现了一个视觉惯导里程计。1. process()线程// thread: visual-inertial odometryvoid process(){ while (true) { std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> m
2020-08-14 18:08:45
442
原创 VINS-mono代码阅读 -- vins_estimator
ROS中节点之间通信机制是基于发布/订阅(publish/subscribe)模型的消息(message)。所以在读一个ROS节点的时候,先看这个节点发布和订阅的消息,以及对应的回调函数。1. main函数int main(int argc, char **argv){ //初始化ros节点 ros::init(argc, argv, "vins_estimator"); ros::NodeHandle n("~"); ros::console::set_logger_lev
2020-08-14 14:41:36
394
原创 VINS-mono代码阅读 --feature_tracker 代码阅读
VINS-mono已经提出有几年了,但是作为经典的视觉惯导结合的SLAM,还是很重要的,即使网上已经出现了大量的代码分析,在阅读前辈的代码同时,总是会有自己的奇奇怪怪的问题,所以有必要在此记录下来。在此,我们只考虑代码中单目的部分,对双目的部分不加解释。1.main函数int main(int argc, char **argv){ //初始化节点 ros::init(argc, argv, "feature_tracker"); ros::NodeHandle n("~");
2020-08-13 22:00:50
342
原创 vector<pair<int, int> >的排序问题
在写代码的时候,需要将一个存了一些key-value的数组进行排序,于是用到了这个东西。#include <utility>#include <iostream>#include <vector>#include <algorithm>using namespace std;typedef pair<int, int> score;vector<pair<int, int> > score_matrix
2020-08-12 20:31:53
4537
原创 C++写文件
在调试程序的时候,有一些关键的信息,需要写在文件中,以便于对代码执行过程进行检查,并对参数进行调整,所以在这里记录下C++写文件的代码。#include<fstream>#include<iostream>using namespace std;int main(){ ofstream OutFile("Test.txt"); OutFile << "This is a Test12!"<<endl; OutFile <&
2020-08-12 20:19:19
362
原创 用opencv提取fFAST角点和SURF描述子这遇到的问题
用opencv提取fFAST角点和SURF描述子这遇到的问题今天在提FAST取特征点和SURF描述子的时候居然出现了问题,我一直以为这玩意上手就干呢,毕竟也是用的opencv的库,现在才发现我还是个弟弟。第一个问题:OpenCV Error: Assertion failed (src.type() == (((0) & ((1 << 3) - 1)) + (((1)-1) << 3)) || src.type() == (((5) & ((1 <<
2020-07-01 23:41:19
227
原创 VINS-mono中图像数据的数据结构
在大概阅读了vins_estimator部分的代码后,被图像数据的各种数据搞的有点懵,所以稍微做一个总结。1.feature_popints在feature_tracker_node 的 img_callback()中创建,包含一帧图像中的特征点信息。在这里插入代码片...
2020-02-10 13:44:32
803
原创 运行PL-VIO
这是中科院自动化所贺一佳博士在Sensors上的文章的开源代码,论文链接,代码链接https://github.com/HeYijia/PL-VIO。第一步要解决的事情就是在自己的电脑上运行起来。按照贺博在readme部分给的步骤就可以。结果如下:LSD_Matches1LSD_Matches2visrviz结果如上图所示,目前对每张图片的具体意义还不清楚,还需要看代码去了解。...
2020-01-26 11:50:06
2452
15
原创 小觅相机深度版运行VINS-mono
刚买了一台小觅深度版相机,拿来试试看效果,决定跑一圈VINS-mono,下面是我运行的过程,记录一下。1. 获取传感器的参数编译官方的SDK。地址:https://mynt-eye-d-sdk.readthedocs.io/zh_CN/latest/sdk/install_ubuntu_src.htmlgit clone https://github.com/slightech/MYNT-E...
2019-12-26 16:26:46
1086
10
原创 找不到头文件--两种include方式区别
今天在改代码的时候,自己加进去一个头文件,然而,在编译的时候,提示找不到头文件,反复确认头文件名称没敲错,路径没问题,然而还是不行,更让我疑惑的是编译器居然可以自动补全头文件的名字。真是奇怪了。后来在这里找到了答案,是关于#include<> 和 #include “” 的使用方法的不同。#include<> 引用的是编译器的类库路径里面的头文件。标准库文件一般都放...
2019-11-30 14:41:21
4707
原创 运行VINS-Mono报错: IMU excitation not enouth!
问题描述闲来无事,跑个VINS-Mono玩玩,按照github给的步骤,一步步进行,没有报错,编译完美结束。但是在运行的时候:![在这里插入图片描述](https://img-blog.csdnimg.cn/20191031094510400.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aH...
2019-10-31 09:48:04
2962
8
空空如也
空空如也
TA创建的收藏夹 TA关注的收藏夹
TA关注的人