当前位置: 首页 > 编程日记 > 正文

VINS-mono详细解读与实现

VINS-mono详细解读

VINS-mono详细解读

前言

Vins-mono是香港科技大学开源的一个VIO算法,https://github.com/HKUST-Aerial-Robotics/VINS-Mono,是用紧耦合方法实现的,通过单目+IMU恢复出尺度,效果非常棒。

感谢他们开源,我从中学到了非常多的知识。源码总共有15000多行,我在通读完程序之后,结合参考文献,把程序背后的算法理论都推导了一遍,总结成了本文,与各位分享。

本文目标读者:对vins-mono有一定了解的SLAM算法工程师。由于程序里有非常多的实现细节,建议读者在读本文前,先读一遍vins-mono的程序。

1.特征点跟踪

首先用cv::goodFeaturesToTrack在第一帧图像上面找最强的150个特征点,非极大值抑制半径为30。新的特征点都有自己的新的对应的id。然后在下一帧过来时,对这些特征点用光流法进行跟踪,在下一帧上找匹配点。然后对前后帧中这些匹配点进行校正。先对特征点进行畸变校正,再投影到以原点为球心,半径为1的球面上,再延伸到深度归一化平面上,获得最终校正后的位置。对于每对匹配点,基于校正后的位置,用F矩阵加ransac来筛选。然后再在匹配上的特征点之外的区域,用cv::goodFeaturesToTrack搜索最强的新的特征点,把特征点数量补上150个。

最后,把剩下的这些特征点,把图像点投影回深度归一化平面上,再畸变校正,再投影到球面上,再延伸到深度归一化平面上,得到校正后的位置。把校正后的位置发送出去。

特征点跟踪和匹配,就是前一帧到这一帧的,一帧帧继承下去。或者生成新的特征点。

2.初始化

2.1外参中的旋转

用机器人手眼标定的方法,计算出外参中的旋转。

其中是陀螺仪预积分得到的,是用8点法对前后帧对应的特征点进行计算得到的。详细见《Monocular Visual-Inertial State Estimation With Online Initialization and Camera-IMU Extrinsic Calibration》。

2.2 SFM

先在关键帧窗口里面,找到第l帧,第l帧与最后一帧有足够的像素位移,并且能用8点法算出旋转和位移。以l帧的姿态为世界坐标系。先从l帧开始与最后一帧进行三角定位,再用pnp估计出下一帧的位姿,下一帧再与最后一帧三角定位得出更多的三维点。重复到倒数第二帧。从l帧开始往第一帧,逐渐帧pnp,再与第l帧进行三角定位得到更多的三维点。每帧pnp时的位姿初值都用上一个关键帧的的位姿。剩下的那些还没有被三角定位的特征点,通过它被观察到的第一帧和最后一帧进行三角定位。

固定住l帧的位置和姿态,固定住最后一帧的位置。因为这时候的图像位姿和点的位置都不太准,所以用ceres统一一起优化图像位姿和三维点位置,优化重投影误差。优化的测量值是,特征点在每帧中被观察到的位置,可以转成重投影误差约束。有关的自变量是,每帧图像的位姿,特征点的三维坐标。

优化完成之后,即用ceres优化出这些关键帧的位姿和地图点后,再用pnp算出在这段时间区域内的所有图像的位姿。每个图像的计算都用下一个关键帧的位姿来当pnp的初值。

程序里面没有求雅克比,而是用自动求导的方法。

2.3 计算陀螺仪的偏移

在2.1中已经根据连续图像的相对旋转算出相机和IMU间的外参旋转了,现在要再根据上一节2.2中的SFM算出来的各帧图像的相对旋转来计算出陀螺仪的偏移。

就是根据前后帧之间的根据陀螺仪预积分出来的旋转与基于SFM图像算出来的旋转转换到IMU坐标系的相对旋转 之间的向量差的两倍。

在程序里面,每次算出的图像的姿态,都会转换成。然后在计算相对IMU的姿态时,就用

这里是采用了近似计算的方法。其实就是把角度的残差转换成了角轴差的形式。详见《从角轴到四元数微分方程》和《on-manifold详细解读》。

这个关于陀螺仪的偏移求导,得到雅克比矩阵。然后再根据高斯牛顿法算出陀螺仪偏移。

算出陀螺仪的偏移以后,对于每一幅图像,利用缓存的每一个IMU数据,重新计算这一段时间的每帧图像对应的预积分,雅克比矩阵和协方差矩阵。

雅克比矩阵的初值是单位阵,协方差矩阵的初值为零。雅克比矩阵每次都乘以一个状态转移矩阵。协方差矩阵每次都左右乘状态转移矩阵,再加上一个噪声矩阵,噪声矩阵就是加速度和陀螺仪噪声所形成的噪声协方差矩阵。

2.4速度,重力和尺度对齐

从2.3之后,认为陀螺仪是准确的,每帧图像的姿态也都是准确的。

速度,重力和尺度对齐,其实就是,每一帧对后一帧的位置,速度的预测值,与当前值的误差。当前值的位置,是SFM位置通过外参计算过来,给的。与这有关的自变量是每帧的速度,重力,尺度。速度,重力,尺度,给的初值都是零。

以第一帧和第二帧为例。

 

因为速度,重力尺度的初值全部都是零,所以第一步,这里残差的计算可以简化成如下。

但雅克比的计算,还是得用原来的表达式算。

虽然给的初值是零,但因为雅克比矩阵里面的元素都是与自变量无关的常数,表示这是线性的,所以只用高斯牛顿法计算一次就可以了。只要数据足够,就能算出比较准确的值。

其中,当前值的位置,是SFM位置通过外参计算过来,给的。机器人手眼标定。

当然,程序里面实际上是把残差转换成了这样的奇怪的坐标系下,结果就会像《technical report》里面的公式18那样子,全部都乘以。但是,我觉得像我上面那样的用机器人手眼标定的方法来表示,会比较方便容易易懂直观明了,更不容易在写表达式的时候出错。

虽然给的初值是零,虽然只用高斯牛顿法计算一次,但只要数据足够,也能算出比较准确的值。每增加一帧,就用高斯牛顿法全部计算一次,速度重力尺度的初值全部都是零。因为反正都可以用高斯牛顿法一次算出结果,所以就不用继承之前的值来优化。

直到优化后的重力接近9.8,比如重力模在[8.8,10.8]区间内。这个方法挺好的,不用先验知识就把重力优化到9.8,结果令人信服。

程序里面,会把关于尺度求导得到的雅克比除以100,这就意味着,尺度这个变量对残差的影响力减弱了100倍。最终为了能消去残差,优化后的尺度会比实际的大100倍。得到后,要再除以100。这么做的目的,应该是要让尺度的精度更高。

每次都直接把这一块的雅克比矩阵和对应的残差,转换成H矩阵的形式,直接加到H矩阵上。为什么要这样做呢?为什么不把全部的雅克比矩阵算好之后再一次性地转换成H矩阵。因为雅克比矩阵太巨大了,而且非常稀疏,里面很多元素都是零。所以,可以直接根据雅克比矩阵的计算表达式知道哪些位置是非零的,然后非零的位置对应相乘加到入H矩阵中对应的位置,即节省存储空间,又能加快计算。

2.5 进一步优化重力

在2.4计算出来一个相对准确的值的基础上,还要再加个约束,即重力的模为9.8。可以有两个方法。

第一个方法是,直接在2.4的残差约束里面增加一个,

但是,如果这样的话,雅克比矩阵里面就有了与自变量有关的元素,就不是线性的了。这样子,计算起来就会比较麻烦,可能需要迭代很多次。

所以,采用方法二。

对模的方向进行优化。2.4的优化仅仅是提供一个较好的重力的方向。

直接给重力一个初值,模的大小就是9.8,方向的初值是2.4中优化后的重力的方向。对这个重力,在切线上调整它的方向。

因为是线性的,所以自变量的初值直接全部设为零。直接一步高斯牛顿法,算出最优值。用来更新

重复上述步骤四次,即更新四次后,就认为这些变量,速度,重力,尺度都已经优化到一个很优的值了。这时候,尺度应该是大于零的。

手动在切线上调整,然后再控制模。其实,就是由自动的非线性优化,改成了手动的线性优化,用来控制迭代次数。

2.6优化地图

用奇异值分解的方法,对每一个特征点都优化重投影误差最小化,优化出它在被观察到的第一帧的相机坐标系下的位置,然后只把深度拿过来用。设它们在被观察到的第一帧的相机坐标系下的齐次坐标为,其它观察帧相对观察第一帧的位姿为。已知它在每帧的投影点,最小化它在每帧的重投影误差。相当于是寻找空间中的一点,与,每帧的光心从投影点发出的射线,距离最小。

与2.2SFM时算出的地图点的区别在于,2.2SFM时算出的地图点是ceres优化出来的关键帧及其对应的地图点。而这里,是固定这段时间的所有的关键帧,这些关键帧的姿态在2.3后期算出陀螺仪偏移后,已经调整过了。奇异值分解,最小化重投影误差,算出地图点的位置。只取在被观察到的第一帧的深度。因为在这时候,各个图像的位姿已经相对比较准确了,所以与2.2中的cere图像位姿和地图点全部优化不同,这里只需要优化地图点就可以了。

在算奇异值分解时,还要增加 模为1的约束。齐次坐标为一个四维的向量,这样子,通过增加模为1的限制,就可以用奇异值分解来优化,这就是这里用齐次坐标的好处。(在程序实际运行的时候,上面方程的左边,还除以了归一化的模,但不会影响计算结果)

基于2.3中的陀螺仪的偏移,重新计算每相邻两个关键帧之前的相对姿态,算的是关键帧之间的预积分。用的是这一段时间缓存的。IMU数据,用中点插值的方法。

然后用2.5中优化出来的尺度,计算出窗口里面的每个关键帧的新的位置,它们相对于第一个关键帧的位姿,把第一个关键帧当作世界坐标系。用2.5中优化出来的速度赋值给每一个对应的关键帧。用尺度来调整每一个特征点的深度。

然后根据重力,计算出当前的世界坐标系相对于水平坐标系的旋转,即第一个关键帧相对于水平坐标系的旋转。然后,把所有关键帧的位置,姿态和速度都转到水平坐标系上。

3.正常的跟踪

每新进来一张图片,上面有跟踪出来的特征点。

对于f_manager中的feature列表中的那些还没有深度的特征点,用奇异值分解计算出一个它的坐标,使得它在它被观察到的每帧图像上的重投影误差最小。它的坐标用在它被观察到的第一帧图像的相机坐标系的深度表示。因为它还有它在被观察到的每帧图像上的归一化坐标。

然后用cere来优化。结合各个关键帧的位姿,各个相机的外参,边缘化的信息,与预积分的误差,每个特征点的重投影误差,回环闭环误差。进行优化。

然后滑动窗口。判断边缘化的条件是,是否进来一个新的关键帧。

如果有边缘化的话,则把窗口中最前面的一个关键帧滑掉。然后把第一次被这个关键帧观察到的特征点,都转移到新的第0个关键帧上。如果没有边缘化的话,则把之前最新的这一帧用进来的最新的这一帧替换掉。

3.1判断是否是关键帧

窗口的大小默认是10。

每当进来一个新的图像帧的时候,首先判断它与窗口里面存储的之前的那一帧的的相对位移,就是与第10帧的特征点的相对位置,用特征点的相对位移来表示。

如果特征点的平均相对位移大于某一个阈值,就认为是一个新的关键帧。就把这个新的关键帧压入到窗口里面,就是压入到第10个位置,然后其它的关键帧都往前移动。第一个位置的关键帧被移出去,边缘化。

如果不是新的关键帧,就把之前的第10帧边缘化掉,这个新的一帧替换成为第10帧。

总之,无论是哪种情况,这个新的一帧肯定都会成为窗口里面的第10帧。

边缘化,是在优化之后才进行的,而且最新的这帧上面观察到的新的特征点并不参与优化。所以,优化的时候,是包括最新的这一帧的11帧的姿态,以及前10帧的特征点在每一帧的投影,包括它们在最新这帧的投影点。所以para_Pose之类的待优化变量的数组长度是11。

3.2创建地图点

对于f_manager中的feature列表中的那些还没有深度的特征点,如果它被之前2帧以上的关键帧观察到过,用奇异值分解计算出一个它的坐标,使得它在它被观察到的每帧图像上的重投影误差最小。它的坐标用在它被观察到的第一帧图像的相机坐标系的深度表示。因为它还有它在被观察到的每帧图像上的归一化坐标。

如果以后要把VINS改用深度相机的话,可以在这里修改。1.信任深度相机,这个点被观察到的第一次的位置就是准确值,直接加入地图点。2.怕深度相机有误差,所以加判断,这个点在它被观察到的连续两帧里面,在世界坐标系中的三维坐标不能相差太大,如果相差太大,就把它的第一帧的观察记录删掉。如果相差不大,就取两个三维坐标的平均值,作为该点的三维位置,加入地图点。3.还可以借鉴ORBSLAM里面的筛选地图点的方法,该点需要在连续几帧中被观察到,并且这连续几帧,观察到的它在世界坐标系中的三维坐标,不能相差太大,就认为它是一个好的点,加入地图点。之后的cere优化中,就不再优化地图点,可以极大地加快优化速度。

3.3cere优化

要优化的目标是各帧的位置,姿态,速度,偏移,以及相机的外参,以及每一个特征点在它被观察到的第一帧的深度。即,要优化的自变量是。

要拟合的目标是,之前边缘化后的先验值,前后帧之间的IMU的预积分值,再加上前后帧之间的偏移的差的约束,每一个特征点的重投影位置。

3.3.1与先验值的残差

其中,prior代表先验值。代表前后帧之间的加速度计的二次积分,代表前后帧之间的加速度计的一次积分。代表前后帧的陀螺仪的一次积分。

另外,要再加上前后帧之间的偏移的差的约束。

所以,在当前的自变量的情况下,与目标的残差为residual,表示如下。

其中,与先验值的残差,为了计算方便,可以再转换到之前的舒尔补之后的残差空间里。用第一状态雅克比。因为在上一次边缘化之前,已经优化之后的残差,舒尔补之后的residual,已经是最小二乘结果,它反馈到自变量上面是接近于零(因为各个方向的量都抵消了)。所以,如果这时候,如果对自变量调整了dx,则与之前的舒尔补后的值的残差会增加J*dx。

其实,在这里,不仅仅是要表示与之前的先验值的差,而是要表示在之前的误差的基础上面叠加上来的误差。因为之前的误差是最小二乘结果,而不是全零。

//每个关键帧的位姿与先验值的不同,会造成残差项增大

Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;

所以,第一部分的雅克比就是之前边缘化后的雅克比,误差就是之前的误差再加上新的误差。

第一部分,要拟合的约束目标是,每帧与之前边缘化后的位姿、速度和偏移不能相差太大。残差用,之前的边缘化后的残差,加上,现在的自变量与之前边缘化后的自变量的相差的值乘以第一状态雅克比。与之相关的自变量是,每帧的位姿,速度和偏移,相机与IMU的相对位姿。残差关于要优化的自变量求导,得到雅克比矩阵。

3.3.2 与预积分的残差

为了简化表示,可以把,里面的A和b矩阵按行分成一块一块的来表示。

从第二部分开始。要拟合的测量值目标是,IMU的加速度计和陀螺仪的预积分。可以转换成实际值与预测值的位姿差来约束。另外还要加上前后偏移接近的约束。与这有关的自变量是,前后帧的位姿,速度,偏移。

其实就是,前一帧基于IMU对后一帧的位置,姿态,速度,偏移,的预测值,与当前值的,误差。

每次迭代之后,重新算误差(需要人为写出算误差的程序)。都会用最新的偏移与之前的偏移的差,重新计算两帧之间的,因为先前预积分的时候已经算好了预积分的值关于偏移的导数。所以,这里直接根据偏移差乘以导数,就能一下调节预积分的值。明明已经有了预积分关于偏移的雅克比了,为什么这里要这样子把偏移单独拿出来算新的预积分,而不是偏移和预积分一起直接优化呢?每迭代一次都要调整一次,为什么不多优化几次,最后再累加呢?因为当新的偏移与旧的偏移相差较大的时候,就不再使用之前预积分关于偏移的雅克比了,需要repropagate,重新计算新的预积分值和雅克比。但程序里把,判断偏移相差较大后重新传播这一块注释了。预积分关于偏移的雅克比矩阵的计算为,状态向量转移或相加,则对应的雅克比矩阵也转移或相加,详见《on-manifold详细解读》。

非线性迭代,每次迭代后,根据残差和雅克比调整自变量的值,再根据调整后的自变量计算出新的残差,再计算出新的雅克比,如此循环。这样子,因为有残差里面的预积分关于偏移的导数,每次迭代后,调节自变量里面的偏移的值后,再计算新的残差时,就方便了。

3.3.3最小化重投影误差

然后,第三部分,要拟合的测量值目标是特征点在各帧图像上的测量位置。可以转换成重投影误差来约束。与这有关的自变量是,该特征点被观察到的第一帧和被观察到的另外一帧的位置和姿态,相机和IMU之间的相对位姿,特征点在第一帧的深度。

其实就是,对每一个地图点的预测的投影点与实际的投影点的误差。

全展开残差项可以表示为,

其中,是当前的原点到测量单位球面点的向量,在球面上的切向单位向量。

它与误差向量相乘,就相当于误差向量在这两个切向向量上的投影。因为

化简后,可以表示为,

其中,链式求导,一步步传导下去。

因为在上述的雅克比矩阵里面,都有自变量了,所以它是非线性的,只能通过优化的方法一步步迭代。

3.3.4边缘化

H*x=JT*residuals,H=JT*J。

基于舒尔补来边缘化。

用奇异值分解的技巧来得到新的雅克比矩阵,作为先验雅克比。由b反过去算残差,作为先验值,在下一次优化时使用。

其实,与《on-mainifold》里面的舒尔补的方法,本质上是一样的。

关于边缘化,还可以再参考http://blog.csdn.net/heyijia0327/article/details/52822104。

3.3.5 回环优化

如果窗口里存在有回环帧的对应帧的话,则先找到对应帧与回环帧的匹配点id和位置,特征点匹配用的是在一定范围内匹配描述子。特征点匹配完之后,然后只保留这些匹配上的id和特征点位置,给front_pose,id用对应帧的id,特征点位置用回环帧的位置。其实,就是把对应帧的id赋给匹配上的回环帧的特征点。然后给front_pose.loop_pose对应帧的姿态作为初值,然后用窗口里面的点的重投影误差来优化front_pose.loop_pose,点的逆深度,点被观察到的第一帧的位姿。

窗口优化之后,比较对应帧与front_pose.loop_pose的yaw角是否相差太大,如果相对yaw角大于30度或相对位置大于20的话,就认为这是错误的回环,删除这个回环。因为这时候,这个对应帧还在窗口里面,还没有滑出去回环闭环。所以,这样也可以及时把DBow找出来的错误的回环,删掉。

其实,就是把回环帧放到窗口里面来优化,优化出回环帧的位姿,然后再算出回环帧和对应帧的相对位姿。然后把这个相对位姿,作为后面的回环4自由度优化的测量值。具体是,优化出回环帧在窗口里面的位置后,算出对应帧相对于回环帧的位姿。

新的关键帧在加入关键帧数据库的时候,作为对应帧,在关键帧数据库里面去找回环帧。如果用DBOW找到回环帧,使用cur_kf->findConnectionWithOldFrame(old_kf, measurements_old, measurements_old_norm, PnP_T_old, PnP_R_old, m_camera);。先用searchByDes 去找匹配点,然后用cv::findFundamentalMat(un_measurements, un_measurements_old, cv::FM_RANSAC, 5.0, 0.99, status);筛选匹配点,再用对应帧自带的地图点结合solvePnPRansac,得到回环帧的位姿PnP_T_old。PnP_T_old作为回环帧位姿loop_pose的初值,结合继承过来的匹配点measurements_old_norm,再传回当前窗口中优化,problem.AddResidualBlock(f, loss_function, para_Pose[start], retrive_data_vector[k].loop_pose, para_Ex_Pose[0], para_Feature[feature_index]);,得到优化后的loop_pose。如果优化后的loop_pose相对对应帧的位姿relative_yaw小于阈值,则认为回环正确。而优化后的loop_pose相对回环帧原来的位姿,为relocalize_r和relocalize_t。

在vins-mono的新版本中,新增加了relocalize_r、relocalize_t,其作用是,在大回环起作用的间隙,用relocalize_r、relocalize_t来对位姿进行及时的修正,以更好地保证输出位姿的准确性,以及关键帧输入到关键帧数据库里时的位姿的准确性。因为以前是要等回环帧的对应帧滑出窗口,大回环优化后,才对这两个位姿进行校正的,而现在可以更及时地修正这些位姿,如果有地方想要最快速地得到准确的位姿的话。new KeyFrame(estimator.Headers[WINDOW_SIZE - 2], vio_T_w_i, vio_R_w_i, cur_T, cur_R, KeyFrame_image, pattern_file);这里面的vio_T_w_i是回环优化时的计算前后关键帧的相对位置时用的,所以用的还是窗口中的位姿。而cur_T,也就是T_w_i,是表示校正后的位姿,在输入的时候,会根据relocalize_r校正,大回环优化后,还再校正关键帧数据库里的所有的关键帧位姿。relocalize_r、relocalize_t也是很巧妙的方法,因为它们是根据回环帧和对应帧的图像的相对位姿算出来的回环偏移,其实大回环优化之后得到的回环偏移correct_t,和这个回环偏移relocalize_t,应该相差不大。大回环主要是优化环中间的那些关键帧的位姿,大回环的主要目的在于修正关键帧图PoseGraph。所以,relocalize_r、relocalize_t一开始就很接近最终大回环优化后的回环偏移correct_t。relocalize_t是把回环帧放到当前窗口下优化,算出来的与原来的偏移;correct_t是大回环优化后的对应帧位姿与它原来的位姿的偏移。correct_t准确但计算慢,relocalize_t计算快速且较准确。

4.回环检测

这里的回环检测,是每3个关键帧检测一帧,相当于是跳两帧。这跟回环检测的速度,和实际关键帧生成的速度,对比有关。因为回环检测的速度总是慢于关键帧生成的速度,所以为了保持回环检测的关键帧不落后于时间,只能跳帧检测。ORBSLAM里面也是这样,但ORBSLAM里面的回环检测判断标准是,一段时间内的关键帧都能匹配上回环,所以ORBSLAM的策略是拿一段时间的关键帧来进行检测。ORBSLAM的回环检测程序,Sleep一段时间,在Sleep的这段时间,收集关键帧,然后开始工作,只针对收集的这些关键帧。工作时,不收集新的关键帧,都跳过。处理完这些关键帧后,又Sleep。

因为回环检测的速度总是慢于关键帧生成的速度,所以为了保持回环检测的关键帧不落后于时间,只能跳帧检测。

窗口里,每3个关键帧,送一个到关键帧数据库里。关键帧数据库里面的每一帧都跟之前的进行检测,看是否有回环。回环检测用的是DBow,这个关键帧上面的用FAST找出来的新的特征点和它在之前被光流跟踪到的特征点,提取描述子,与历史描述子匹配。这帧的描述子以及对应的特征点,跟历史上的描述子以及对应的特征点进行匹配,得到匹配上的特征点。

如果有回环,就等这个对应帧从窗口里滑出,再回环闭环。

找出回环最早开始的帧,然后把这帧的位姿设为固定。

回环闭环的约束条件是,与优化前的相对位姿不能差太大,每帧与前5帧的相对位姿。回环帧和对应帧的相对位姿的权重是5。回环闭环里面,优化的都是4自由度的姿态,回环帧与闭环帧,每帧与它的前5帧的相对姿态。

优化完后,在最后一个对应帧那里,再把世界坐标系也转换一下,然后把剩下的关键帧都转换一下。

回环闭环优化部分的测量值是,回环帧与对应帧的基于图像算出的相对4自由度姿态,就是relative_t和relative_yaw,就是回环帧的loop_pose在窗口中优化后,相对窗口中的对应帧的位姿。每帧图像与它前5帧的相对4自由度姿态。约束为,预测值和测量值之间的差。与此有关的自变量是,每帧图像的4自由度位姿。就像一个项链一样,一串地拉过来。

注意的是,回环帧与对应帧的基于图像算出的相对4自由度姿态的权重是5,为了平衡每帧图像与它前5帧的相对4自由度姿态。对应帧的前后对它的拉力要相同。

假设是第0帧和第m帧回环闭环了。

在这里,究竟是使用还是,我认为是一样的。前者是把下一帧相机的原点转换到上一帧相机的yaw角的水平坐标系下,以此为测量值,预测值与测量值的残差,关于yaw角求导。后者是把把下一帧的相机的原点转换到上一帧的坐标系下,预测值与测量值的残差,关于yaw角求导。但这里的yaw角都是指在水平世界坐标系里面的yaw角。最终优化的结果应该是一样的。像这种,只是把残差从一个直角坐标系转换到另外一个直角坐标系,结果应该是一样的。但是,如果是另外一些转换,那结果就可能不一样了,比如非直角坐标系,各维度的有不同的缩放尺度,或者维度变换,那样子的话,优化结果就会不一样。

这里使用程序里面的表示,。程序里面没有求雅克比,而是用自动求导的方法。

回环检测之后的处理,非常有创新点。一般的,回环检测之后,是要根据回环检测结果,把窗口关键帧的位姿都转换掉。但实际上,是可以不管窗口里面的关键帧的,可以认为窗口里面的关键帧的世界坐标系相对真实的世界坐标系发生了偏移和转换。只需要在输出位姿的时候,乘以这个转换就可以了,窗口里面的位姿还是原来的位姿。这样子,就可以避免对窗口的位姿进行大幅度调整,也不需要重新计算雅克比,协方差矩阵。输出的结果是应用所需要的,修正过的结果。

为了输出最新的位姿,需要把新缓存进来的IMU数据也都积分起来。更新实时的最新的位姿。然后,每新进来一个IMU数据,就在之前的基础上预测一下。因为IMU数据都是在载体坐标系的,所以,可以先在窗口的坐标系里积分,然后最后再转换到真实的世界坐标系,也可以直接把坐标转换到真实的世界坐标系,然后再积分。最后都是得到在真实的世界坐标系里面的位姿,然后每新进来一个IMU数据,就在之前的基础上预测一下。这两种方法是一样的。

5.流程图

VIN-SLAM实现转载自博客:https://www.cnblogs.com/Jessica-jie/p/6950604.html方便自己使用参考学习

香港科技大学的VINS_MONO初试

期刊分享SLAMVINS-Mono:一种稳健的单目视觉惯性状态估计器 上

期刊分享SLAMVINS-Mono:一种稳健的单目视觉惯性状态估计器 下

第一部分是测量处理前端,负责提取并跟踪每个新图像帧的特征,及帧间IMU数据预积分。
第二部分是初始化过程,提供了激活非线性系统的必要初始值,如姿态、速度、重力向量、陀螺仪偏差及3D特征位置。
第三部分为非线性图优化,通过联合优化所有的视觉、惯性信息求解滑窗内的状态。
另一部分运行另一个线程,负责闭环检测及姿态图优化。

VINS-mono详细解读

VINS-Mono is a real-time SLAM framework for Monocular Visual-Inertial Systems. It uses an optimization-based sliding window formulation for providing high-accuracy visual-inertial odometry. It features efficient IMU pre-integration with bias correction, automatic estimator initialization, online extrinsic calibration, failure detection and recovery, loop detection, and global pose graph optimization. VINS-Mono is primarily designed for state estimation and feedback control of autonomous drones, but it is also capable of providing accurate localization for AR applications. This code runs on Linux, and is fully integrated with ROS. For iOS mobile implementation, please go to VINS-Mobile.

https://github.com/HKUST-Aerial-Robotics/VINS-Mono

1.运行环境

Ubuntu 14.04, ROS Indigo, OpenCV 3.1.0, Eigen 3.2.0,Ceres Solver

2. Build VINS-Mono on ROS

在此之前,由于我的opencv有两个版本-OpenCV 2.4.9和OpenCV 3.1.0,引起冲突,让我很是苦恼...但最终还是解决啦啦啦啦。。。

记录在此http://www.cnblogs.com/Jessica-jie/p/6959309.html

Clone the repository and catkin_make:

cd ~/catkin_ws/src
git clone git@github.com:HKUST-Aerial-Robotics/VINS-Mono.git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash

结果:   这5个package均有可执行文件生成,在/home/wj/catkin_ws/devel/lib/里

3. Performance on EuRoC dataset

3.1Download EuRoC MAV Dataset.Although it contains stereo cameras, we only use one camera.

3.2 Open three terminals, launch the vins_estimator , rviz and play the bag file respectively. Take MH_05 as example:

[1]roslaunch vins_estimator euroc.launch    ///estimationroslaunch benchmark_publisher publish.launch     ///ground-truth
[2]roslaunch vins_estimator vins_rviz.launch 等同于---rosrun rviz rviz -d /home/wj/catkin_ws/src/VINS-Mono/config/vins_rviz_config.rviz
[3]rosbag play YOUR_PATH_TO_DATASET/MH_05_difficult.bag ---rosbag play /home/wj/Downloads/dataset/EuRoC/ROS_bag/MH_05_difficult.bag 

注意:***.launch要在其工作空间执行.

wj@wj-Inspiron-5437:~$ cd catkin_ws
wj@wj-Inspiron-5437:~/catkin_ws$ source ~/catkin_ws/devel/setup.bash
wj@wj-Inspiron-5437:~/catkin_ws$ roslaunch vins_estimator euroc.launch wj@wj-Inspiron-5437:~$ cd catkin_ws
wj@wj-Inspiron-5437:~/catkin_ws$ source ~/catkin_ws/devel/setup.bash
wj@wj-Inspiron-5437:~/catkin_ws$ roslaunch benchmark_publisher publish.launch

结果图:

解析:

wj@wj-Inspiron-5437:~/catkin_ws$ roslaunch vins_estimator euroc.launchSUMMARY
========PARAMETERS* /feature_tracker/config_file: /home/wj/catkin_w...* /feature_tracker/vins_folder: /home/wj/catkin_w...* /rosdistro: indigo* /rosversion: 1.11.20* /vins_estimator/config_file: /home/wj/catkin_w...* /vins_estimator/vins_folder: /home/wj/catkin_w...NODES/feature_tracker (feature_tracker/feature_tracker)vins_estimator (vins_estimator/vins_estimator)auto-starting new master
process[master]: started with pid [4194]
ROS_MASTER_URI=http://localhost:11311setting /run_id to d4e07290-fea5-11e7-92da-bc8556c5ff8b
process[rosout-1]: started with pid [4207]
started core service [/rosout]
process[feature_tracker-2]: started with pid [4211]
process[vins_estimator-3]: started with pid [4222]
[ INFO] [1516537385.770250050]: init begins
[ INFO] [1516537385.783868675]: Loaded config_file: /home/wj/catkin_ws/src/VINS-Mono/feature_tracker/../config/euroc/euroc_config.yaml
[ INFO] [1516537385.786188731]: Loaded vins_folder: /home/wj/catkin_ws/src/VINS-Mono/feature_tracker/../config/../
[ WARN] [1516537385.786329200]:  Optimize extrinsic param around initial guess!
[ INFO] [1516537385.786765223]: Extrinsic_R : 
2.22045e-16          -1           01 2.22045e-16           00           0           1
[ INFO] [1516537385.786810735]: Extrinsic_T : 
-0.02 -0.06  0.01
[ WARN] [1516537385.786861452]: waiting for image and imu...
[ WARN] [1516537385.830039004]: LOOP_CLOSURE true
voc file: /home/wj/catkin_ws/src/VINS-Mono/feature_tracker/../config/..//support_files/brief_k10L6.bin
loop start load vocabularyloop closure init finish

wj@wj-Inspiron-5437:~/catkin_ws$ roslaunch benchmark_publisher publish.launch
SUMMARY
========PARAMETERS* /benchmark_publisher/data_name: /home/wj/catkin_w...* /rosdistro: indigo* /rosversion: 1.11.21NODES/benchmark_publisher (benchmark_publisher/benchmark_publisher)ROS_MASTER_URI=http://localhost:11311core service [/rosout] found
process[benchmark_publisher-1]: started with pid [6334]
[ INFO] [1520410994.189896520]: Loaded data_name: /home/wj/catkin_ws/src/VINS-Mono/benchmark_publisher/config/MH_05_difficult/data.csv
load ground truth /home/wj/catkin_ws/src/VINS-Mono/benchmark_publisher/config/MH_05_difficult/data.csv
[ INFO] [1520410994.284550074]: Data loaded: 22212

4.AR Demo

4.1 Download the bag file, which is collected from HKUST Robotic Institute. For friends in mainland China, download from bag file.

4.2 Open three terminals, launch the ar_demo, rviz and play the bag file respectively.We put one 0.8m x 0.8m x 0.8m virtual box in front of your view.

 roslaunch ar_demo 3dm_bag.launchroslaunch ar_demo ar_rviz.launchrosbag play /home/wj/Downloads/dataset/ar_box.bag ---YOUR_PATH_TO_DATASET

结果图:

参考资料:

VINS-Mono代码注释以及公式推导:https://github.com/QingSimon/VINS-Mono-code-annotation
不依赖ROS的VINS:https://github.com/heguixiang/Remove_ROS_VINS
https://blog.csdn.net/wangshuailpp/article/details/78461171
https://blog.csdn.net/houlianfeng/article/details/79525854
https://blog.csdn.net/houlianfeng/article/details/79626657
https://blog.csdn.net/houlianfeng/article/details/79713333
https://www.cnblogs.com/ilekoaiq/p/8836970.html

相关文章:

mysql+mycat搭建稳定高可用集群,负载均衡,主备复制,读写分离

数据库性能优化普遍采用集群方式&#xff0c;oracle集群软硬件投入昂贵&#xff0c;今天花了一天时间搭建基于mysql的集群环境。 主要思路 简单说&#xff0c;实现mysql主备复制-->利用mycat实现负载均衡。 比较了常用的读写分离方式&#xff0c;推荐mycat&#xff0c;社区活…

【UVA/Codeforces】1584 Circular Sequence / 792B Counting-out Rhyme(就是一个圈儿...)

https://vjudge.net/problem/UVA-1584 1584 Circular Sequence 输入一个字符串&#xff0c;可以以字符串中任意一个字母作为起始&#xff0c;输出字典序最小的那个字符串 两种方法&#xff0c;一种是设两个标记 【样例输入】CGAGTCAGCT 【样例输出】AGCTCGAGTC 一开始 an…

一个free异常引发的异常

有同事反馈说自己的线程不工作&#xff0c;查看堆栈发现其打印如下&#xff1a; #0 0x00007f291f72e7be in __lll_lock_wait_private () from /lib64/libc.so.6 #1 0x00007f291f6c2e4e in _L_lock_9925 () from /lib64/libc.so.6 #2 0x00007f291f6c1101 in free () from /li…

欧拉角和旋转矩阵相互转换

目录 1.参考资料 2.变换矩阵/F/H的svd分解或者旋转矩阵、平移矩阵求解 3. 欧拉角和旋转矩阵可同样表示刚体在三维空间的旋转&#xff0c;下面分享这两者互相转换的方法和核心代码 1.参考资料 2.变换矩阵/F/H的svd分解或者旋转矩阵、平移矩阵求解 欧拉角转旋转矩阵 欧拉角…

【Codeforces】 2A - Winner (map)

http://codeforces.com/problemset/problem/2/A So, if two or more players have the maximum number of points (say, it equals to m) at the end of the game, than wins the one of them who scored at least m points first. 所以只有一个只有一个map不行&#xff0c;…

[译]Godot系列教程一 - 场景与节点

场景(Scene)与节点(Node) 简介 先设想有那么一瞬间你自己不再是一名游戏开发者了&#xff0c;而是一名大厨&#xff01; 你的装备换成了一套大厨的制服。不要考虑制作游戏的事情&#xff0c;你现在的职责是为你的顾客创建新的可口的食谱。 那么&#xff0c;大厨是怎样创建食谱的…

EOS与以太坊有哪些区别?

想知道更多关于区块链技术知识&#xff0c;请百度【链客区块链技术问答社区】 链客&#xff0c;有问必答&#xff01;&#xff01;EOS与以太坊有哪些区别&#xff1f; 以太坊是一个专门为开发和运行去中心化应用&#xff08;DAPP&#xff09;搭建的智能合约平台&#xff1b;EOS…

图像特征点检测与匹配评价准则——量化

欢迎转载&#xff0c;转载请注明出处&#xff0c;谢谢&#xff01; 目前图像匹配中&#xff0c;局部特征匹配占据了绝大部分&#xff0c;常用的局部特征匹配方法有Harris、SIFT、SURF、ORB等等&#xff0c;不同的特征点检测和匹配方法尤其独特的优势和不足&#xff1b; 特征点匹…

path,classpath

1.path作用. 在环境变量里面配置 winr 打开cmd qq窗口就弹开了。 2.classpath是java里的选项。 java执行java类的时候&#xff0c;会去看这个java类是否在classpath路径下。不在就不能编译转载于:https://www.cnblogs.com/shapeOfMyHeart/p/5975686.html

【Codeforces】401C Team (01010110...)

http://codeforces.com/contest/401/problem/C 题目中&#xff0c;n表示0的个数&#xff0c;m表示1的个数&#xff0c;要求两个0不能连续&#xff0c;三个1不能连续 还要判断能否输出满足要求的序列&#xff0c;不满足输出-1 满足条件以后徐瑶分情况讨论 当1比0多&#xff…

表白这件事,比解 bug 要难多少?

情人节快乐&#xff01;我是可爱无敌的阿里妹。 今天是个粉红色日子&#xff0c;我们来聊聊和技术无关的“技术活”&#xff0c;比如&#xff1a; “如何表白&#xff1f;” 当技术人碰上动心的姑娘&#xff0c;他的浪漫开关就打开了。 在代码王国里劈荆斩刺的王子&#xff0c;…

特征点匹配+特征检测方法汇总

特征点匹配特征检测方法汇总特征提取与匹配---SURF&#xff1b;SIFT&#xff1b;ORB&#xff1b;FAST&#xff1b;Harris角点匹配方法匹配函数1. OpenCV提供了两种Matching方式&#xff1a;• Brute-force matcher (cv::BFMatcher) //暴力方法找到点集1中每个descriptor在点…

元数据驱动的微服务架构(上)

本次分享有两个部分&#xff1a; 微服务架构需要元数据 介绍微服务与元数据的关系。 一、微服务架构需要元数据 企业IT架构已经发展了多个阶段&#xff0c;一方面是服务化架构的发展&#xff0c;在SOA阶段主要解决应用间集成问题&#xff0c;但随着企业业务的发展&#xff0c;…

【Codeforces】427B Prison Transfer(别让罪犯跑了...)

http://codeforces.com/problemset/problem/427/B 从一串数字中选出连续的长度为c的子串&#xff0c;且子串中的每一个数都不能大于t&#xff0c;问这样的子串有多少个 TLE&#xff0c;看看n的范围就知道了&#xff0c;哎呀呀&#xff0c;有点chun #include <iostream>…

PHPUnit实践三(构建模块化的测试单元)

本系列教程所有的PHPUnit测试基于PHPUnit6.5.9版本&#xff0c;Lumen 5.5框架 目录结构 模块下的目录是符合Lumen的模块结构的如&#xff1a;Controllers、Models、Logics等是Lumen模块目录下的结构目录如果有自己的目录同级分配即可&#xff0c;如我这里的Requests 整体结构 ├…

SLAM笔记(五)光束平差法(Bundle Adjustment)

1.什么是光束平差法 前边的八点法&#xff0c;五点法等可以求出闭式解的前提是已经知道确切的点对。但实际情况中往往存在大量的噪声&#xff0c;点与点不是精确地对应甚至出现一些错误匹配。 光束平差法由Bundle Adjustment翻译得来&#xff0c;有两层意思&#xff1a; 对场…

【Code forces】63B Settlers' Training

http://codeforces.com/problemset/problem/63/B 给你一串数字&#xff0c;直到所有数字都变为k为止&#xff0c;相同的数为一组&#xff0c;在一次中&#xff0c;所有不同的数都加1 1 2 2 3 → 2 2 3 4 → 2 3 4 4 → 3 4 4 4 → 4 4 4 4 #include <ios…

[elixir! #0007] [译] 理解Elixir中的宏——part.5 重塑AST by Saša Jurić

上一章我们提出了一个基本版的deftraceable宏&#xff0c;能让我们编写可跟踪的函数。宏的最终版本有一些剩余的问题&#xff0c;今天我们将解决其中的一个——参数模式匹配。 今天的练习表明我们必须仔细考虑宏可能接收到的输入。 问题 正如我上一次暗示的那样&#xff0c;当前…

vue-cli3环境变量与分环境打包

第一步 : 了解环境变量概念 我们可以根目录中的下列文件来指定环境变量: .env # 在所有的环境中被载入 .env.local # 在所有的环境中被载入&#xff0c;但会被 git 忽略 .env.[mode] # 只在指定的模式中被载入 .env.[mode].local # 只在指定…

SLAM闭合回环————视觉词典BOW小结

在目前实际的视觉SLAM中&#xff0c;闭环检测多采用DBOW2模型https://github.com/dorian3d/DBoW2&#xff0c;而bag of words 又运用了数据挖掘的K-means聚类算法&#xff0c;笔者只通过bag of words 模型用在图像处理中进行形象讲解&#xff0c;并没有涉及太多对SLAM的闭环检测…

【Codeforces】53D Physical Education (有点像冒泡)

http://codeforces.com/problemset/problem/53/D 从上面所给的序列变成下面所给的序列 交换的时候只能交换相邻的两个数字 输出每一步的交换方法&#xff0c;输出的是该元素在序列中的位置&#xff08;第一个数的位置是1&#xff09; 不要求输出步数最少的那一种方法 当同…

js脚本冷知识

js中有个很恶心的写法。比如这个var adsf(function(){})();这样的写法&#xff0c;主要是原生的&#xff0c;能在dom元素加载完之后实现自动加载这个脚本文件到里面去。可以验证这个&#xff08;function(){.......&#xff09;&#xff08;&#xff09;;在.......里面可以直接…

Python中is同一性运算符和==相等运算符区别

2019独角兽企业重金招聘Python工程师标准>>> 在区分is和这两种运算符区别之前&#xff0c;需要知道Python中对象包含的三个基本要素&#xff0c;分别是&#xff1a;id(身份标识)、type(数据类型)和value(值)。 比较对象的value(值) 是python标准操作符中的比较操作符…

C++实现十大排序算法(冒泡,选择,插入,归并,快速,堆,希尔,桶,计数,基数)排序算法时间复杂度、空间复杂度、稳定性比较(面试经验总结)

排序算法分类 内部排序算法又分为基于比较的排序算法和不基于比较的排序算法&#xff0c;其分类如下&#xff1a; 比较排序&#xff1a; 直接插入排序 希尔排序 &#xff08;插入&#xff09; 冒泡排序 快速排序 (交换) 直接选择排序 堆排序&#xff08;选择&#…

32位处理器是什么意思

问题描述&#xff1a;朋友那个32位处理器&#xff0c;2的32次方算出来的单位是不是应该是4294967296位&#xff08;bit&#xff09;吧&#xff0c;怎么就成字节了呢&#xff1f;单位错了&#xff0c;那个32位是指32位地址总线&#xff0c;而不是32位数据。地址的数量单位是个&a…

【Codeforces】913C Party Lemonade (贪...)。

http://codeforces.com/contest/913/problem/C 这个题和以前见过的有点不一样&#xff0c;可以重复选择&#xff0c;这个有点emmm 首先将a数组优化&#xff0c;举个例子&#xff0c;如果1L20元&#xff0c;2L50元&#xff0c;那么将a[1]赋值为40&#xff0c;而不是50。 之后…

GDB 调试 Mysql 实战(二)GDB 调试打印

背景 在 https://mengkang.net/1328.html 实验中&#xff0c;我们通过optimizer_trace发现group by会使用intermediate_tmp_table&#xff0c;而且里面的的row_length是20&#xff0c;抱着"打破砂锅问到底"的求学精神&#xff0c;所以想通过 gdb 调试源码的方式看这个…

移动端AR的适用分析(二)

移动端AR的适用分析&#xff08;二&#xff09;1. 单目SLAM难点 2. 视觉SLAM难点 3. 可能的解决思路 单目slam的障碍来自于理论和实践两个方面。理论障碍可以看做是固有的&#xff0c;无法通过硬件选型或软件算法来解决的&#xff0c;例如单目初始化和尺度问题。实践问题包括计…

新的理念、 新的解决方案、 新的Azure Stack技术预览

Jeffrey Snover 我们很高兴地宣布︰ Azure Stack Technical Preview 2&#xff08;TP2&#xff09;已发布&#xff01;我们朝着向您的数据中心提供Azure服务能力的目标又更近一步。自发布第一个技术预览版&#xff08;TP1&#xff09;以来&#xff0c;我们访问了很多用户&…

【HDU】1084 What Is Your Grade? (结构体 sort)

http://acm.hdu.edu.cn/showproblem.php?pid1084 题目的关键&#xff1a; 1、Note, only 1 student will get the score 95 when 3 students have solved 4 problems. If you can solve 4 problems, you can also get a high score 95 or 90 (you can get the former(前者)…