深圳网站建设_河北企业网站建设技术_静态网页制作_外贸营销系统

手搓视觉定位算法：第二章

默认读者对视觉slam和视觉定位有一定的基础
没有的话可以参考《视觉十四讲》或者作者的另外一个专栏
有一点基础就行

文章目录

手搓视觉定位算法：第二章
前言
一、理论基础
二、代码实现
- 2.1 BA优化问题
- 2.2 三角化路标点
- 2.3 Ceres优化求解
三、实验结果
总结

前言

本章节的目的是改进第一章的单目定位算法
依然是帧到帧的，但是会换成BA来求解
第一章链接：点我
因为还不涉及建图，所以这里还不能叫做slam

一、理论基础

简单回顾一下BA：原理就是重投影误差

假设有n个路标 $P_i$ （3D点），他们在k个图像中的二维坐标为 $P_i^k$ （2D点），k个图像分别的位姿是 $T_k$
那么BA的作用就是调整 $P_i$ 和 $T_k$ ，让 $P_i$ 在投影后能够尽量和 $P_i^k$ 重合

那有人要问了，这没什么可优化的啊，都知道谁是谁了，求解一下PNP不就好了

解释一下：这里3D点的估计是利用三角化或者其他方法估计的深度，是不准确的，2D点是通过帧间特征点匹配获得的，PNP当然会用，但是PNP不考虑3D点的优化，只优化位姿

但是3D点的位置并不正确，所以要用BA来联合调整

（其实这里了解的人已经发现了，如果只做帧到帧的BA，其实发挥不了BA的作用）

二、代码实现

流程基本和第一章差不多，这里就只讲一些不同

2.1 BA优化问题

本文用的是Ceres，如果大伙会g2o也可以自己写，作者确实懒得学了

那么这里就构建一个重投影误差的struct

struct ReprojectionError {ReprojectionError(double observed_x, double observed_y, const cv::Mat& K): observed_x(observed_x), observed_y(observed_y), K(K) {}template <typename T>bool operator()(const T* const camera, const T* const point, T* residuals) const {// 接收相机的位姿camera和路标的3D点pointT fx = T(K.at<double>(0, 0));T fy = T(K.at<double>(1, 1));T cx = T(K.at<double>(0, 2));T cy = T(K.at<double>(1, 2));const T* rotation = camera;const T* translation = camera + 3;T p[3];ceres::AngleAxisRotatePoint(rotation, point, p);p[0] += translation[0];p[1] += translation[1];p[2] += translation[2];// 计算重投影坐标 T u = fx * p[0] / p[2] + cx;T v = fy * p[1] / p[2] + cy;// 计算误差residuals[0] = u - T(observed_x);residuals[1] = v - T(observed_y);return true;}static ceres::CostFunction* Create(const double observed_x, const double observed_y, const cv::Mat& K) {// 创建残差块，输入观测到的2D点，用来初始化这个struct，后面再接受3D点算残差return (new ceres::AutoDiffCostFunction<ReprojectionError, 2, 6, 3>(new ReprojectionError(observed_x, observed_y, K)));}double observed_x;double observed_y;const cv::Mat& K;
};

这个结构体能够为每组2D-3D点构建残差

先用Create来输入2D的观测点，再添加对应的相机位姿和对应的3D路标点，即可优化问题

2.2 三角化路标点

因为是单目，所以路标点的三角化只能联合两帧图像来做

第一章中我们是用本质矩阵计算了第二帧的位姿，那么基于这个位姿，我们就可以进行三角化

回忆一下，我们通过本质矩阵获得了第二帧相机的位姿R，t，以及两组匹配点

那么就可以进行三角化：

        std::vector<cv::Point2f> pre_pts_norm, cur_pts_norm;cv::undistortPoints(pre_pts, pre_pts_norm, K, cv::Mat());cv::undistortPoints(cur_pts, cur_pts_norm, K, cv::Mat());cv::Mat P1 = cv::Mat::eye(3, 4, CV_64F);cv::Mat P2 = cv::Mat::eye(3, 4, CV_64F);R.copyTo(P2(cv::Rect(0, 0, 3, 3)));t.copyTo(P2(cv::Rect(3, 0, 1, 3)));// 三角化这一组匹配点对cv::Mat points_4d;cv::triangulatePoints(P1, P2, pre_pts_norm, cur_pts_norm, points_4d);// 归一化处理，不然这些点的尺度可能不一致std::vector<cv::Point3f> points_3d;for (int i = 0; i < points_4d.cols; i++) {cv::Mat point = points_4d.col(i);point /= point.at<float>(3); points_3d.emplace_back(point.at<float>(0), point.at<float>(1), point.at<float>(2));}

2.3 Ceres优化求解

        ceres::Problem problem;double camera1[6] = {0, 0, 0, 0, 0, 0}; double camera2[6]; cv::Rodrigues(R, cv::Mat(3, 1, CV_64F, camera2));camera2[3] = t.at<double>(0);camera2[4] = t.at<double>(1);camera2[5] = t.at<double>(2);std::vector<std::array<double, 3>> points_3d_double;for (const auto& point : points_3d){// Ceres求解需要double类型，不能用float，所以这里转换一下points_3d_double.push_back({point.x, point.y, point.z});}for (size_t i = 0; i < points_3d_double.size(); ++i) {// 创建残差块，设置2D点ceres::CostFunction* cost_function = ReprojectionError::Create(cur_pts[i].x, cur_pts[i].y, K);// 输入3D点和相机位姿，计算残差，并输入问题中// 如果拓展到多帧后，这里的循环就没这么简单了problem.AddResidualBlock(cost_function, nullptr, camera2, points_3d_double[i].data());}// 求解问题ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_SCHUR;options.minimizer_progress_to_stdout = true;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);