SLAM第7章:三角测量

  第6章介绍了使用2维点求解相机位姿的方法。二维点丢失了距离信息,能不能求得这些距离能?三角测量可以帮到我们。

1. 三角测量

  假设空间点在两个相机坐标系中的三维坐标是PPPP',如果这两个相机坐标系的变换关系用RRt\vec t表示,那么:
P=RP+t.P = RP' + \vec t.

  假设nnnn'分别是PPPP'对应的归一化成像平面坐标,zzzz'分别是PPPP'到相机光心平面的距离,那么:
zn=zRn+t.zn = z'Rn' + \vec t.

  上式两边分别与nn求外积:
zn×n=zn×Rn+n×t.zn × n = z'n × Rn' + n × \vec t.

  写成矩阵相乘的形式:
zn^n=zn^Rn+n^t.(7.1.1)zn^{\hat{\quad}} n = z'n^{\hat{\quad}}Rn' + n^{\hat{\quad}} \vec t. \tag{7.1.1}

  公式左边等于0,于是:
zn^Rn+n^t=0.(7.1.2)z'n^{\hat{\quad}}Rn' + n^{\hat{\quad}} \vec t = 0. \tag{7.1.2}

  当我们知道像素坐标时,可以利用相机内参知道归一化成像平面坐标,由第6章还可以知道RRt\vec t,于是公式(7.1.2)(7.1.2)就可以解得zz'。知道了zz'根据公式(7.1.1)(7.1.1)就可以解得zz

  讨论:在第6章已经说过,我们只知道二维坐标,实际距离信息是无法求得的。第6章求得的平移向量t\vec t的模总是1,缩放系数α\alpha无法获得。同样,这里我们求得的zzzz'也是缺少同样的那个缩放系数α\alpha。由于是同一个系数,所以如果能知道两个相机的平移距离(光心距离),那么平移向量和三角测量的结果乘这个系数就可以得到真实距离。

  第6章讲的是使用二维坐标求变换,第7章将的是使用二维坐标求距离,这些内容可以用于单目视觉SLAM的初始化:刚开始建图的时候,没有地图,只有两幅图像。第一幅图像对应的第一个相机坐标系为世界坐标系,求第二个相机坐标系到世界坐标系的位姿变换,这个位姿变换称为第二个相机的位姿。我们可以根据第6章求位姿变换,根据第7章求点的距离。这样就可以得到每个二维点在世界坐标系中的坐标。存储的这些三维点(可以包含三维坐标、特征点信息等)称为地图。如果不知道缩放系数α\alpha,得到的就是一个比例尺未知的地图。

2. 使用OpenCV的三角测量

#include <iostream>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

Point2d cam2pix(const Point3d& p, const Mat& K) {
	return Point2d((p.x * K.at<double>(0, 0)) / p.z + K.at<double>(0, 2),
		       (p.y * K.at<double>(1, 1)) / p.z + K.at<double>(1, 2));
}

Point2d pix2nor(const Point2d& p, const Mat& K) {
	return Point2d((p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
		       (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

Point2d cam2nor(const Point3d& p, const Mat& K) {
	return pix2nor(cam2pix(p, K), K);
}

void triangulation(const vector<Point2d> pts1,
		   const vector<Point2d> pts2,
		   const Mat& T1, const Mat& T2, const Mat& K) {
    
	Mat pts_4d;
	cv::triangulatePoints(T1, T2, pts1, pts2, pts_4d);

	for(int i=0; i<pts_4d.cols; i++) {
		Mat x = pts_4d.col(i);
		x /= x.at<double>(3, 0);
		Point3d p(x.at<double>(0, 0), x.at<double>(1, 0), x.at<double>(2, 0));
		cout << i << ": " << p << endl;
	}
	// 输出的点正好是我输入的三维坐标,这是因为平移向量(0, -1, 0)的模正好是1。如果模是x,则结果要乘x。
}

int main() {
	Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
	vector<Point2d> pts1;
	vector<Point2d> pts2;

	pts1.push_back(cam2nor(Point3d(2, 3, 1), K));
	pts1.push_back(cam2nor(Point3d(2, -2, 3), K));
	pts1.push_back(cam2nor(Point3d(3, -2, 2), K));
	pts1.push_back(cam2nor(Point3d(1, -3, 1), K));
	pts1.push_back(cam2nor(Point3d(4, 0, 2), K));
	pts1.push_back(cam2nor(Point3d(0, -8, 3), K));
	pts2.push_back(cam2nor(Point3d(-4, 2, 1), K));
	pts2.push_back(cam2nor(Point3d(1, 2, 3), K));
	pts2.push_back(cam2nor(Point3d(1, 3, 2), K));
	pts2.push_back(cam2nor(Point3d(2, 1, 1), K));
	pts2.push_back(cam2nor(Point3d(-1, 4, 2), K));
	pts2.push_back(cam2nor(Point3d(7, 0, 3), K));

	Mat T1 = Mat::eye(3, 4, CV_64F);
	Mat T2 = (Mat_<double>(3, 4) <<
		 0, 1, 0,  0,
		-1, 0, 0, -1,
		 0, 0, 1,  0);

	triangulation(pts2, pts1, T1, T2, K);
}

  我们给cv::triangulatePointscv::triangulatePoints的参数是二维的归一化成像平面坐标和两个相机的位姿T1T1T2T2

  运行结果:
在这里插入图片描述  只需交换一下参数顺序就可以求出第一个相机坐标系坐标的距离。这里的运行结果和实际值相同,也是因为我们的平移向量(0,1,0)(0,-1,0)的模正好为1。

发布了162 篇原创文章 · 获赞 24 · 访问量 5万+
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 大白 设计师: CSDN官方博客

分享到微信朋友圈

×

扫一扫,手机浏览