关于libviso2姿态解算部分的代码解释（参考MT.QUEEN的博客）

__JUNBO__

于 2019-02-23 10:09:05 发布

阅读量824

点赞数

分类专栏：视觉slam 文章标签：视觉里程计

本文链接：https://blog.youkuaiyun.com/qq_34622953/article/details/87888931

版权

博客详细介绍了libviso2中姿态估计的过程，首先通过匹配点获取3D坐标，然后利用RANSAC算法进行迭代，以最小化重投影误差。在RANSAC循环中，采用牛顿高斯方法更新参数，该过程涉及雅克比矩阵的填充和计算重投影误差。整个过程遵循《视觉SLAM14讲》中的牛顿高斯方程，以确保位姿估计的准确性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

在位姿部分运行之前，前后两帧的位姿匹配已经完成，有p_match文件

estimationMotion函数：完成位姿估计任务，并返回R，t的值rt_delta
vector rt_delta = poseCompute.estimationMotion(p_matched);

estimationMotion函数,首先通过左右两帧的匹配，完成对于当前帧中匹配点的３维坐标表示
// project matches of previous image into 3d//这一段是从双目得到相机坐标系下空间点的坐标表示
for (int32_t i = 0; i<N; i++) {
double d = max(p_matched[i].u1p - p_matched[i].u2p, 0.0001f);
X[i] = (p_matched[i].u1p - param.calib.cu)*param.base / d;
Y[i] = (p_matched[i].v1p - param.calib.cv)param.base / d;
Z[i] = param.calib.fparam.base / d;
}

调用getRandomSample，完成随机取样
vector<int32_t> active = getRandomSample(N, 3);