再谈单应性矩阵及位姿分解

最近在研究无人驾驶的定位问题，想利用车载摄像头提取的车道线、配合厂商提供的GNSS+IMU(非ADMA级别方案，存在一定误差)，还有地图厂商提供的HD map(包含车道线元素)纠正定位。也就是说不依赖于视觉slam构造的特征图层，尝试下在GNSS+IMU定位不精确的情况下，仅利用前置摄像头的车道线，纠正lateral以及heading angle的误差。

偶然之间，搜到一篇论文“Lane Level Localization; Using Images and HD Maps to Mitigate the Lateral Error”.

论文大致思想通过初始定位位置搜索到地图对应的车道线元素，然后假设车身在longitudinal方向的错位尚可，在摄像头提取的左和右车道线上分别提取2个对共4个点，取车头前方D米以及2D米的视觉车道线，同理在地图的对应位置上也提取这么4个点，然后求图像平面到地图平面的单应性变换关系，再分解单应性矩阵得到R和T作为修正汽车定位的依据。(注意：厂商提供的地图和视觉车道线，可能是N阶曲线的系数，自己采样即可；另外，提取的车道线越远误差越大，但是地图误差不会随着距离增加而加大)

这篇论文的假设在一定情况下是成立的，汽车虽然行径方向误差较大，上述车道线采样的点和地图采样的点实际上不一定匹配，但是只要是车头前方不远处的点，都不影响纠正lateral误差。

本文打算总结下我所了解单应性矩阵，因为针对不同场景的单应性矩阵，矩阵分解有不同方法。

1.什么是单应性矩阵

平面的单应性即为一个平面到另一个平面的映射。这个平面可以是图像平面，也可以是三维世界中的平面(z=0)

数学定义为，x=Mx'

单应性矩阵解法，一般就是DLT 4点法。如果给定的匹配点比较多，可以考虑RANSAC之类的，opencv有很方便的库

2.单应性矩阵有哪些场景

单应性变换适用于如下两种场景，这种变换矩阵都叫做单应性矩阵。但是他们是不同的。

a.图像平面与物理平面

如下例子相机与物理平面不必是正对的，可以想想下无人机定位航拍，俯视角度做定位

b.图像平面与图像平面

两个相机平面看着同样的物理平面，但是相机在不同位置拍摄

3.如何分解R和T

a.针对图像平面与物理世界平面

上述公式是套用了相机模型的投影公式，理论上旋转和平移应该是，

但是由于噪声，实际情况是h1和h2并不正交，所以有如下处理

这里，解是唯一的，也是易于理解的。有人这么干的，

void cameraPoseFromHomography(const Mat& H, Mat& pose)
{pose = Mat::eye(3, 4, CV_32FC1);      // 3x4 matrix, the camera posefloat norm1 = (float)norm(H.col(0));  float norm2 = (float)norm(H.col(1));  float tnorm = (norm1 + norm2) / 2.0f; // Normalization valueMat p1 = H.col(0);       // Pointer to first column of HMat p2 = pose.col(0);    // Pointer to first column of pose (empty)cv::normalize(p1, p2);   // Normalize the rotation, and copies the column to posep1 = H.col(1);           // Pointer to second column of Hp2 = pose.col(1);        // Pointer to second column of pose (empty)cv::normalize(p1, p2);   // Normalize the rotation and copies the column to posep1 = pose.col(0);p2 = pose.col(1);Mat p3 = p1.cross(p2);   // Computes the cross-product of p1 and p2Mat c2 = pose.col(2);    // Pointer to third column of posep3.copyTo(c2);       // Third column is the crossproduct of columns one and twopose.col(3) = H.col(2) / tnorm;  //vector t [R|t] is the last column of pose
}

b.针对图像平面之间的情况

假设3D平面方程为（一般式）

那么有，

根据两个相机的成像模型，我们有

我们对第二个图像画面做如下扩展，

上述公式只是证明了，这种变换也是单应性变换。接下来怎么求这个R和t呢？比较麻烦

数值法：

解析法：(opencv函数就是这种)

虽然能做R和T的分解，但是对于位移T来说单目视觉没有实际的scale深度信息，只能知道一个位移的方向。

所以那个论文被我同事鄙视了，lateral方向不能给出明确的偏移量。关于位移量的问题，可以看如下仿真代码。

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>int main() {// set up a virtual camerafloat f = 100, w = 640, h = 480;cv::Mat1f K = (cv::Mat1f(3, 3) <<f, 0, w/2,0, f, h/2,0, 0,   1);// set transformation from 1st to 2nd camera (assume K is unchanged)cv::Mat1f rvecDeg = (cv::Mat1f(3, 1) << 45, 12, 66);cv::Mat1f t = (cv::Mat1f(3, 1) << 100, 200, 300);std::cout << "-------------------------------------------\n";std::cout << "Ground truth:\n";std::cout << "K = \n" << K << std::endl << std::endl;std::cout << "rvec = \n" << rvecDeg << std::endl << std::endl;std::cout << "t = \n" << t << std::endl << std::endl;// set up points on a planestd::vector<cv::Point3f> p3d{{0, 0, 10},{100, 0, 10},{0, 100, 10},{100, 100, 10}};// project on both camerasstd::vector<cv::Point2f> Q, P, S;cv::projectPoints(p3d,cv::Mat1d::zeros(3, 1),cv::Mat1d::zeros(3, 1),K,cv::Mat(),Q);cv::projectPoints(p3d,rvecDeg*CV_PI/180,t,K,cv::Mat(),P);// find homographycv::Mat H = cv::findHomography(Q, P);std::cout << "-------------------------------------------\n";std::cout << "Estimated H = \n" << H << std::endl << std::endl;// check by reprojectionstd::vector<cv::Point2f> P_(P.size());cv::perspectiveTransform(Q, P_, H);float sumError = 0;for (size_t i = 0; i < P.size(); i++) {sumError += cv::norm(P[i] - P_[i]);}std::cout << "-------------------------------------------\n";std::cout << "Average reprojection error = "<< sumError/P.size() << std::endl << std::endl;// decompose using identity as internal parameters matrixstd::vector<cv::Mat> Rs, Ts;cv::decomposeHomographyMat(H,K,Rs, Ts,cv::noArray());std::cout << "-------------------------------------------\n";std::cout << "Estimated decomposition:\n\n";std::cout << "rvec = " << std::endl;for (auto R_ : Rs) {cv::Mat1d rvec;cv::Rodrigues(R_, rvec);std::cout << rvec*180/CV_PI << std::endl << std::endl;}std::cout << std::endl;std::cout << "t = " << std::endl;for (auto t_ : Ts) {std::cout << t_ << std::endl << std::endl;}return 0;
}

输出结果：

-------------------------------------------
Ground truth:
K =
[100, 0, 320;
0, 100, 240;
0, 0, 1]rvec =
[45;
12;
66]t =
[100;
200;
300]-------------------------------------------
Estimated H =
[0.04136041220427821, 0.04748763375951008, 358.5557917287962;
0.05074854454707714, 0.06137211243830468, 297.4585754092336;
8.294458769850147e-05, 0.0002294875562580223, 1]-------------------------------------------
Average reprojection error = 0-------------------------------------------
Estimated decomposition:rvec =
[-73.21470385654712;
56.64668212487194;
82.09114210289061][-73.21470385654712;
56.64668212487194;
82.09114210289061][45.00005330430893;
12.00000697952995;
65.99998380038915][45.00005330430893;
12.00000697952995;
65.99998380038915]t =
[10.76993852870029;
18.60689642878277;
30.62344129378435][-10.76993852870029;
-18.60689642878277;
-30.62344129378435][10.00001378255982;
20.00002581449634;
30.0000336510648][-10.00001378255982;
-20.00002581449634;
-30.0000336510648]

As you can see, there is correct estimation of rotation vector among the hypothesis, and there's a up-to-scale correct estimation of translation.

关于尺度问题，它和像素点的取值有关系。大家可以把那个3D 平面坐标，放大/缩小10倍，观察translation的区别

4.单应性矩阵与对极几何的关系

这里援引高翔的话，

单应性在 SLAM 中具重要意义。当特征点共面，或者相机发生纯旋转的时候，基础矩阵的自由度下降，这就出现了所谓的退化（ degenerate）。现实中的数据总包含一些噪声。这时候如果我们继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵 F 和单应矩阵H，选择重投影误差比较小的那个作为最终的运动估计矩阵。

再谈单应性矩阵及位姿分解相关推荐

基础矩阵，本质矩阵，单应性矩阵讲解
ORB-SLAM点云地图中相机的位姿初始化,无论算法工作在平面场景,还是非平面场景下,都能够完成初始化的工作.其中主要是使用了适用于平面场景的单应性矩阵H和适用于非平面场景的基础矩阵F,程序中通过一个 ...
单应性矩阵的理解及求解3
http://www.sohu.com/a/223594989_100007727 前面文章<从零开始学习「张氏相机标定法」(一)成像几何模型>中我们已经得到了像素坐标系和世界坐标系下的坐 ...
Baxter抓取物块——基于单应性矩阵（一）
最近在学习Baxter双臂机器人,做了一个简单的单臂物块抓取来作为起步阶段的成果.大体思路很简单:让手臂到达指定的起始位置,获取图像,找到物块的轮廓并计算单应性矩阵,再计算手臂实际需要移动的偏移量,最 ...
RANSAC算法的单应性矩阵讲解
还可以参考:https://blog.csdn.net/lhanchao/article/details/52849446 我们已经得到了像素坐标系和世界坐标系下的坐标映射关系: 其中,u.v表示像素 ...
单应性矩阵的求解过程及应用
http://www.sohu.com/a/223594989_100007727 前面文章<从零开始学习「张氏相机标定法」(一)成像几何模型>中我们已经得到了像素坐标系和世界坐标系下的坐 ...
计算机视觉学习笔记（四）homography 单应性矩阵的理解及求解
单应性矩阵的理解及求解 1. 齐次坐标(Homogeneous Coordinate) 一幅2D图像上的非齐次坐标为(x,y),而齐次坐标为(x,y,1),也可以写成(x/z,y/z,1)或(x,y, ...
opencv求两张图像光流_OpenCV单应性矩阵发现参数估算方法详解
点击上方蓝字关注我们微信公众号:OpenCV学堂关注获取更多计算机视觉与深度学习知识单应性矩阵计算函数与应用 OpenCV在通过特征描述子完成描述子匹配之后,会得到一些关键点对,我们会把这些关键 ...
相机标定-opencv单应性矩阵实现平面坐标标定（kinect v1）
opencv单应性矩阵实现平面坐标标定说明一.使用单应性矩阵的原因二.标定原理三.findHomography 函数与 getPerspectiveTransform函数的区别 1.两者联系 ...
《增强现实：原理、算法与应用》读书笔记（1）基础矩阵、本质矩阵与单应性矩阵
<增强现实:原理.算法与应用>读书笔记(1) 入坑增强现实,学长让我先把这本书看懂,看了一小半,确实有很多对数学要求挺高的地方,所以打算写个读书笔记,分享一些体会. 第三章:实景的三维结构 ...

再谈单应性矩阵及位姿分解

再谈单应性矩阵及位姿分解相关推荐

最新文章

热门文章