诗筱涵的博客

飞PX4的SLAM定位时，包括飞T265时，想查看飞控端ODOMETRY这个mavlink消息，确认位姿传入正确，注意需要把飞控参数的MAV_ODOM_LP设置为1。现在清楚为什么以前飞T265时，有时有ODOMETRY这个mavlink消息有时没有了。利用视觉或运动捕捉系统进行位置估计 | PX4 自动驾驶用户指南 (v1.14)

vins是基于滑窗优化的，ORBSLAM3是基于共视图优化的，所以ORBSLAM里面没有边缘化这种。所说的中期数据关键，或者较大视差，可能就是指共视关键帧可以距离时间相对较远，视差相对大一些。BA优化共视关键帧，应该是ORBSLAM3精度高的原因。这里有个博文也有这么总结。

为了看清具体时延，可以进一步放大，对比时间戳差值，如下图所示，可以看到横轴的时间相差大概在200ms左右，可以粗略认为是SLAM位姿的时延是200ms左右，如果我们飞PX4的SLAM定位时，想修改PX4飞控中的EKF2_EV_DELAY参数，使得它更为准确，也可以基于此进行填写，EKF2_EV_DELAY参数含义是Vision Position Estimator delay relative to IMU measurements，它的可修改范围是0-300ms。我们理想中的无人机定点应该是纹丝不动的。

我们在三个具有挑战性的数据集上的实验:KITTI、EuRoC和KAIST-VIO，以及两个高度精确的SLAM后端:Full-BA和ICE-BA表明，Jetson-SLAM是可用的最快的精确和GPU加速SLAM系统。相反，基于滤波的方法实现了高效率，但牺牲了精度。受到基于优化的方法中的舒尔补的启发，作者充分利用了用于姿态和地标构建的高维残差模型中固有的稀疏结构，以实现EKF的高效性。之前自己总结的计算受限平台上的SLAM，主要这么改，前端，改为直接法(甚至稀疏直接法)或者GPU加速，后端，改为滤波的形式。

从机载端电脑或者其他信息可以得到视觉SLAM输出的位姿，可以通过mavlink 协议的VISION_POSITION_ESTIMATE或者ODOMETRY消息输入给飞控，区别在于VISION_POSITION_ESTIMATE只包含位置信息，而ODOMETRY还可以得到速度、姿态和姿态角速度等。

二进制安装realsense2_camera功能包之后，虽然会附带着二进制安装对应的librealsense，但是这个二进制安装的librealense是不包含realsense-viewer的，此时如果我们也还想同时用realsense-viewer，可以再单独二进制安装。

我们平常使用OrinNX时，其CPU频率默认不是最高的，而且是会根据具体情况进行变动的，默认是自动变频模式，我们在NX上跑一些CPU算力较高的程序时，可以把CPU频率提至最高，并设置为定频再进行使用。在OrinNX，我们可以在 /sys/devices/system/cpu/cpufreq/ 目录下看到policy0 policy4这两个文件，policy0对应CPU 1-4，policy4对应CPU 5-6(8G OrinNX)或CPU 5-8(16G OrinNX)

vins-fusion和ORBSLLAM算力对比，我们可以看一个基础的数字，vinsfusion里面yaml文件设置每帧提取特征点数量的时候默认是150个，实际跑时可以调整到50个，ORBSLAM里面yaml文件设置的每帧提取特征点数量是一千多个。

我们平常基于PX4室外实飞SLAM定位时，想评估分析下SLAM估计的位置如何，我们可以借助于GPS数据作为位置真值进行简单对比分析（RTK更好，但是对于很多普通学生而言可能没有RTK，而GPS模块很便宜），这里介绍下，如何基于px4飞控日志进行SLAM估计位置和GPS经纬度的对比分析，这里的前提是无人机上装有GPS模块并接在了PX4飞控上，但是位置来源选择的是来自于视觉，不是GPS，也就是基于SLAM位姿进行定位飞行，而飞控日志会同时也记录下GPS的经纬高信息。10-7经度或纬度。

不管是vins-fusion还是ORBSLAM3，如果接收到的是rgb图像，也是先转为灰度图再进行处理的。vins-fusion如果收到rgb图也是转为灰度图再处理的。ORBSLAM3是会把图像转为灰度图去处理的。

kalibr相机IMU联合标定的重投影误差小于0.2是来自于kalibr官方wiki，原话是重投影误差在0.1-0.2个像素之间是好的标定。

不像ORBSLAM前端就发出位姿话题了，vins虽然也有前端tracking，但是前端tracking输出的是比如特征点匹配结果，并没有相机帧初始位姿估计，相机帧初始位姿估计放在了后端estimator.cpp里。Estimator::processMeasurements()函数。processImage函数部分内容。

在我自己i5的笔记本电脑上，注意订阅的图像话题需要改。最后还是卡住挂掉了，我Bag包以0.2倍速播放的，i5 cpu 8G内存。opencv3.3.1装好后我都没有额外源码编译装cv_bridge。内存占用感觉也是一直在缓慢增长，但是没有FAST-LVIO那么快，感觉可能是我ubuntu笔记本上的opencv是3.2的原因。livox功能包装后，接着catkin_make，有报错。我改在一台opencv版本3.3.1的ubuntu上编译。再catkin_make r3live就编译通过了。

Nga 和NgaWalk 有在Preintegrated::IntegrateNewMeasurement函数里更新协方差的时候用到，协方差也就是信息矩阵，程小六的书里有讲（这感觉跟vins里面影响IMU预积分权重类似啊 ），Preintegrated::IntegrateNewMeasurement函数就是预积分计算函数（可以看这里的注释。ORBSLAM3初始化对配置文件里的IMU标定参数（噪声和随机游走）这么敏感，是拿那些参数参与实际的IMU预积分运算了，vins里面那些参数只是影响IMU预积分权重。

你自己后来标定D435i的时候，就基本没有重新单独标定IMU了，就一直用的最开始标定的D435i的IMU标定结果进行的双目IMU联合标定，当时觉得算是一种偷懒，只要最后双目IMU联合标定结果的重投影误差小于0.2就认为OK了，除非在没有重新标定imu的情况下，双目IMU联合标定的重投影误差始终达不到0.2，这个时候我才建议说去重新标定一下IMU，但是实际似乎用之前的IMU标定结果，双目IMU联合标定的重投影误差基本也都可以达到0.2，当然我后来真正亲手标定的次数也不是很多了。下面这段代码别人也有相应解释（

D435i的/camera/imu没有发出来，可能是因为librealsense realsense-ros D435i的固件版本不匹配的原因。确实用2.50.0的realsense-viewer就读不出，用其他版本的realsense-viewer就读得出imu数据。要想完全卸载之前二进制安装过的librealsense（有两种方式，一种二进制直接安装librealsense，一种二进制安装realsense2_camera间接安装了librealsense）在合众NX上遇见过这样的情况，所以记录下来。

vins订阅的MARS数据集里的imu话题注意订阅大疆飞控的imu话题/dji_osdk_ros/imu，不要订阅livox激光雷达的imu话题/livox/imu，因为/livox/imu里的加速度值是普通加速度值的十分之一，会导致vins一直没法初始化过去，终端一直报misalign visual structure with IMU。vins-fusion跑港大MARS数据集只能跑单目IMU的。

这种可能出现在比如，用港大的MARS数据集跑vins，如果订阅Livox的imu话题/livox/imu，vins就会终端显示那样，初始化不过去，因为/livox/imu里的加速度值是普通加速度值的十分之一。可以改订阅MARS数据集里大疆飞控的imu话题。跑vins初始化时出现misalign visual structure with IMU很可能是因为订阅的IMU话题里加速度的值不对或者偏差太大。也是受到这个人回答的启发，这个人看了下也是港科大的。

飞基于飞控imu的vins的时候，实际是通过ros做的时间同步，所以不用专门弄时间同步了， ros时间同步，图像和Imu时间还有微小偏移靠kalibr标定解决，所以vins的配置文件里里面就有参数设定图像和imu两者间的延时，应该就是针对ros时间同步后还存在的延时或者说偏移。

看这个公式就好理解了，为什么要估计bias和重力。噪声可能没法避免就不估计了，IMU的偏置影响不大就不估计了，所以最后就只剩下估计陀螺仪的bias和重力了，为了得到准确的角速度和不包含重力的机体加速度。加速度计不光测的运动加速度，还测了重力加速度，所以需要把这个重力加速度去掉才能得到正确的运动加速度。需要估计重力方向，并把重力从总的加速度里面移除出去，这样才能得到真正相机的加速度。IMU的预积分也是可以得到相对旋转关系的，预积分不光光是平移注意。由于VIO的非线性，有一个好的初始值是它稳定运行的基础。

但是其实可以更少，Rt是刚体运动，刚体自由度是6，减去一根尺度，那自由度是5，R的自由度是3，t的自由度，因为绝对尺度求不出来，所以自由度为2不是3.所以理论上我们最少只需要五对点就可以把R和t给求出来了。看着我理解了本质矩阵，就是Rt相乘的一个矩阵！求本质矩阵其实就是求Rt!R*t总共有9个数字，但是，E绝对的尺度是不知道的，因为那个方程，多大相乘都是为0，所以只需要8个点，也就是8点法！（我看计算机视觉中的多视图几何里面也是有讲基本矩阵的计算的，本质矩阵是基本矩阵的一部分，pnp也是要求解本质矩阵吧）

十四讲里的边缘化和vins里的边缘化是一个东西一个公式一个原理，只是十四讲里面边缘化是只想算相机位姿，marg掉的特征点的位置，vins里的边缘化是只想算滑窗内的优化变量，滑窗丢弃的变量不算，这个样子。marg的对象不同，理论是同一个理论。十四讲里的边缘化就和贺一家这里讲的边缘化一样。

这可能才是关键，舒尔补是这种把一个矩阵变为上三角或者下三角的方法，应该是的，乘以一个舒尔补就可以变了！因为随着滑动窗口的移动，那些变量也在更新，对应的海森矩阵也更新，舒尔补后的矩阵也在更新，舒尔补后的海森矩阵分解出来的雅可比和残差也就跟着在变。这个步骤不正和下下面的那个边缘化的步骤对应么，计算机视觉life的vins课里面也讲到了的，也是这个意思，现在看到官方正式描述了。边缘化的优化肯定也是不算算出δx，也会不断更新优化变量的值，去减小残差，更新优化变量的值这就相当于起作用了，在优化位姿等等的了。

这个在崔华坤和马朝伟的vins资料里面都有说到。

在/opt/ros/foxy/share/realsense2_camera/launch目录下，找到rs_launch.py，修改rs_launch.py使得其启动D435i可以发出30hz的分辨率640*480双目灰度图话题和200hz的/camera/imu话题，给vins节点订阅使用。修改后的可以发出30hz的分辨率640*480双目灰度图话题和200hz的/camera/imu话题和rs_launch.py如下。修改rs_launch.py前需要先赋予权限。

OrinNX上部署运行ROS2版本的vinsfusiongpu 20240429OrinNX ubuntu20.04上前提已经装好ros foxy。

包括我现在看单目深度估计或者基于深度学习的里程计，确实就是深度和位姿或者光流和位姿一起估计，不是单一的，这个层面再去反看自监督单目深度估计为什么那么弄可能就好理解了。) (GeoNet：基于无监督学习的深度、光流和相机位姿的联合估计 CVPR2018)上面的需要大量样本训练，没有大量样本时实现无监督的光流和深度的联合估计。深度学习可以得到稠密光流，稠密光流可以得到稠密的特征点匹配，通过三角测量可以得到稠密的深度。【华中科技大学杨欣教授：“基于视觉的深度估计与定位”-哔哩哔哩】

我自己实现PLVIO输出位姿了，它的关键在于得自己先建好一个txt，并且yaml文件里的output_path里面路径也得带上文件名，因为也是看代码发现的，发现代码并没有对应文件名，再想到其他的yaml文件output_path里面路径最后是个txt，我也是在想要建什么类型的文件，看到其他的是建的txt，所以我也建个txt，再运行起来，这个txt里面有位姿数据了。因为也检查了，代码没有问题，有专门输出位姿到文件的代码，也被调用了的。把PL-VIO的位姿保存到txt文件的方法。

第三页有点大，直接复制不上来，是自己再分两次截图放上来的。是可以自己完全推出来的，包括带有旋转矩阵李代数的求导。

20221007时自己画的PL-SLAM分支图。

速度得到提升，因为只采用显著方向上的线段进行跟踪，没有使用所有的线段进行跟踪。不过从这里也可以看到，并不是需要所有数据集上都好，按照实际情况来！可以看出线的误差函数上也可以下功夫改进。这里有点的三角化和结构线的三角化。人家论文题目里面就包含了线三角化。激光SLAM通常分为两大类。这篇论文我搜了下在这可以看。这里说到了结构线的三角化。结构信息辅助的SLAM。

噪声可能没法避免就不估计了，IMU的偏置影响不大就不估计了，所以最后就只剩下估计陀螺仪的bias和重力了，为了得到准确的角速度和不包含重力的机体加速度。vins-mono还有专门的一步优化重力向量，我觉得可以加到这里来弄，曼哈顿世界的Z轴。加速度计不光测的运动加速度，还测了重力加速度，所以需要把这个重力加速度去掉才能得到正确的运动加速度。需要估计重力方向，并把重力从总的加速度里面移除出去，这样才能得到真正相机的加速度。由于VIO的非线性，有一个好的初始值是它稳定运行的基础。我什么还要单独弄个重力的优化呢？

vins里面这个UNIT_SPHERE_ERROR宏定义来选择平面相机模型还是球面相机模型，需要注意下。

aruco又称为aruco标记、aruco标签、aruco二维码等，其中 CharucoBoard GridBoard AprilTag 原理相通，只是生成字典不同，而AprilTag用于机器人领域或可编程摄像头比较多，而aruco CharucoBoard GridBoard则用于AR应用或智能眼镜比较多，都是用于实现精准定位，如无人机降落时就可以使用它实现精准降落。ArUco marker是一种汉明码方格图。

以下面这个变换矩阵为例，一个变换矩阵可以有两种理解方式，可以理解为机体坐标系在世界坐标系下的位姿，最右边的写法可以体现这个含义，也可以说是从机体系到世界系的变换矩阵，最左边的写法可以体现这个含义，因为变换矩阵右边乘的是变换前的坐标系坐标，左边得到的是变换后的坐标系坐标，所以写为Twb正好顺序一致，念的时候也是先念b再念w。对于坐标系间的旋转角度是90度的倍数时，可以直接观察坐标轴的对应关系或者取坐标轴的特定坐标值的方式快速确定坐标系间的旋转矩阵。变换矩阵用Eigen::Isometry3d类型。

Tbw Tb->w 不是世界系到机体系的变换矩阵，是机体系到世界系的变换矩阵，把机体系变到世界系！求机体系在世界系下的位姿，不是把世界系变到机体系，而是机体系变到世界系，比如机体系原点，在机体系下坐标是（0，0，0），但是在世界系下就是一个非零值，这个坐标是世界系下的坐标不是机体系下的。eigen里也默认是左乘，左乘是从最右边向左边乘，依次乘以左边的。所以变换矩阵相乘按照左乘(坐标是列向量)是写成下面这样。

以前的这种方法不能叫赋值，可能导致错误。

vcs-import < ./SchurVINS/dependencies.yaml 这一步下不下来可以把之前部署过的svo的工作空间下的对应功能包复制过去 ，因为我看这12个功能包它下载链接也是从对应官方Github仓库下的，没有自己更改。应该是我的地址问题，这里是kgithub，不是github，之所以出现kgithub是我复制的之前部署svo时的工作空间下的包，当时部署svo时可能就改为kgithub了，github的一个镜像网站，当时还能用，现在可能不能用了。我先把这个地址改为Github吧。

下面这个相机系在机体系下的旋转矩阵就可以写成这样，直接根据x y z轴的对应关系，比如机体系x的正对应相机系y的负，所以旋转矩阵里对应机体系x和相机系y的地方就是-1。所以其实从这个层面看，不管是左乘还是右乘，只是表示方式不同，对应的实际旋转是一样的。坐标系间的旋转是90度的倍数，其旋转矩阵是可以一眼写出来的，不管左乘还是右乘。只需要分别取三个在三个坐标轴上的点就可以了。有相关性的就有值，因为没有缩放，所以不是1就是-1。

最后我把代码丢给chatgpt而且说报错是这样，chatgpt分析出是我赋值没有写在main函数里的原因，我把赋值写在main函数里面后果然就没有does not name a type的报错了。Eigen::Matrix3d类型用