VINS-mono 学习之三角化

参考：https://blog.csdn.net/u013517182/article/details/52151960

https://blog.csdn.net/kokerf/article/details/72844455

感谢前辈们的分享，对我理解VINS代码有很大的帮助。

三角化（Triangulate）是计算机视觉中的重要的一部分，通过特征匹配的二维坐标点和图像的位姿（R，t）得到三维空间点的位置。

放一张示意图:

根据几何关系，假设两帧图像上匹配特征点的齐次坐标为x, x'。三维空间点齐次坐标记为X。两帧图像对应的投影矩阵为P，P’。可以得到：

x = P X x' = P' X

使用叉乘得到AX = 0的形式（可以使用SVD的方法计算）：

x×(P X) = 0 x'×(P' X) = 0

其中对x = (x, y, z)分析，将P和P’按行拆开，得到：

由此可以得到三个方程，由于第三个方程可以由前两个方程得到，因此只需要考虑前两个方程。每对匹配的特征（x和x'）都会得到四个方程，表示为AX =0 的形式，

通常由于匹配的特征点对较多，这是一个超定方程，一般无法求得精确解，我们其实需要求一个使AX最小的解。通过对A的SVD分解，最后得到的是A的最小奇异值对应的单位奇异矢量，若X = (x, y, z, w)，深度则为z/w。

在VINS-mono中，对滑动窗口中所有帧中depth未知的特征点进行三角化。对应于代码如下：

void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])
{for (auto &it_per_id : feature){it_per_id.used_num = it_per_id.feature_per_frame.size();if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))continue;if (it_per_id.estimated_depth > 0)continue;int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;ROS_ASSERT(NUM_OF_CAM == 1);Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);int svd_idx = 0;Eigen::Matrix<double, 3, 4> P0;Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];P0.leftCols<3>() = Eigen::Matrix3d::Identity();P0.rightCols<1>() = Eigen::Vector3d::Zero();for (auto &it_per_frame : it_per_id.feature_per_frame){imu_j++;Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];Eigen::Vector3d t = R0.transpose() * (t1 - t0);Eigen::Matrix3d R = R0.transpose() * R1;Eigen::Matrix<double, 3, 4> P;P.leftCols<3>() = R.transpose();P.rightCols<1>() = -R.transpose() * t;Eigen::Vector3d f = it_per_frame.point.normalized();svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);if (imu_i == imu_j)continue;}ROS_ASSERT(svd_idx == svd_A.rows());Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();double svd_method = svd_V[2] / svd_V[3];//it_per_id->estimated_depth = -b / A;//it_per_id->estimated_depth = svd_V[2] / svd_V[3];it_per_id.estimated_depth = svd_method;//it_per_id->estimated_depth = INIT_DEPTH;//ROS_INFO("before <> feature id %d , start_frame %d, used num %d, depth %f ", it_per_id.feature_id, it_per_id.start_frame, it_per_id.used_num,it_per_id.estimated_depth);  //zssif (it_per_id.estimated_depth < 0.1){it_per_id.estimated_depth = INIT_DEPTH;}}
}

VINS-mono 学习之三角化相关推荐

VSLAM中的特征点三角化
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:紫薯萝卜 https://zhuanlan.zhihu.com/p/103694374 本文仅做 ...
PCL点云使用贪婪三角化进行曲面重构
一.PCL点云平滑和法线估计题目:https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486705&idx=1& ...
CGAL4.4_2D限定三角化及网格（2D Conforming Triangulations and Meshes）
1限定三角化 1.1 定义 Delaunay三角剖分: Delaunay三角剖分是 Boris Delaunay 于1934 年提出的.它具有一些优秀的性质,例如说它最大化三角剖分T中所有三角形的最小 ...
python计算机视觉学习第三章——图像到图像的映射
目录引言一. 单应性变换 1.1 直接线性变换算法 1.2 仿射变换二. 图像扭曲 2.1 图像中的图像 2.2 分段仿射扭曲 2.2 图像配准三.创建全景图 3.1 RANSAC(随机一致性 ...
今夜喜雨 | 直线线性三角化理论
雨夜,用了两个小时的时间终于推导了直线三角化的公式.直线三角化? 是的,即通过两帧的匹配直线得到空间直线的端点,如果端点严格匹配那么就是点的三角化,你可以从<SLAM十四讲>一书或者其他的 ...
Polyworks脚本开发学习笔记(三)-TREEVIEW进阶操作
Polyworks脚本开发学习笔记(三)-TREEVIEW进阶操作移动/交换对象的顺序移动对象的顺序 TREEVIEW FEATURE MOVE ( 1,2 ) 将索引号为1和2的特征交换位置 T ...
三维点云质心与三角化 — python open3d
1 质心介绍质心概念与重心的计算方式相同.如下所示: (1) 即 ...
PCL中GreedyProjection三角化算法简介与示例
文章目录前言一.PCL点云三角化 1.1 Delaunay三角剖分 1.2 贪婪三角化二.程序示例总结前言 Delaunay三角剖分最初应用于2维领域,而与Greedy三角化算法的结合,使之 ...
delauney 三角化
欢迎关注更多精彩关注我,学习常用算法与数据结构,一题多解,降维打击. 给定平面内三角网格进行最优delauney三角化空圆性如果一个三角网格中任意一对三角形满足空圆性,则该三角网格为delaun ...

VINS-mono 学习之三角化

VINS-mono 学习之三角化相关推荐

最新文章

热门文章

VINS-mono 学习之 三角化

VINS-mono 学习之 三角化相关推荐

最新文章

热门文章

VINS-mono 学习之三角化

VINS-mono 学习之三角化相关推荐