前言
三角测量是在已知相机参数和图像中匹配点的情况下,求解这些匹配点对应的空间点三维座标的方法。针对单目与双目系统,三角测量的使用方法有所不同。双目视觉测距原理参见:双目视觉测距原理深度剖析:一个被忽略的小问题,与双目相机相比,单目相机拍摄的两张图片没有固定的位姿,也就没有准确的基线,所以其三角化是指根据相机的运动来估计特征点的深度信息。
这里主要介绍直接线性变换法(Direct Linear Transform,DLT)。
直接线性变换法
设三维空间点在世界座标系下的齐次座标为,相应的在两个视角的投影点分别为和,它们在相机座标系下的座标为:
两幅图像对应的相机投影矩阵分别为(3*4维),
其中,分别是投影矩阵的第1-3行;分别是投影矩阵的第1-3行;在理想情况下,有:
对于,在其两侧分别叉乘其本身,可知:
即:
可以得到:
其中,第三个方程可以由前两个进行线性变换得到,因此每个视角可以提供两个约束条件,联合第二个视角,可以得到:
其中,
针对上述方程,当视角点数较少且不存在外点时可直接对矩阵进行SVD分解来求解,得出座标;当存在外点(错误的匹配点),则采用RANSAC(随机一致性采样)的方法进行估计。
当点数多于2个时,还可以采用最小二乘法进行求解;由于每次观测可以提供两个约束条件,因此当存在次观测时,,使用最小二乘法求解,即是求:
对进行SVD分解:
其中,为奇异值,且由大到小排列,正交。
则可知,当时,公式(4)将取得最小值,且最小值为。
上述DLT求解中,是已知和投影矩阵求解对应点的三维空间座标;除此之外,DLT还可以用来求解PnP问题,只不过已知条件变成了和,推导过程也与上述过程类似。
代码
如下代码是直接对矩阵进行SVD分解的方式来求解。有关最小二乘法求解及代码,可参考:《视觉SLAM十四讲》ch13.3:三角化公式推导及代码详解
#include <iostream>
#include <math/matrix_svd.h>
#include "math/vector.h"
#include "math/matrix.h"
using namespace std;
int main(int argc, char* argv[])
{
math::Vec2f p1;
p1[0] = 0.289986; p1[1] = -0.0355493;
math::Vec2f p2;
p2[0] = 0.316154; p2[1] = 0.0898488;
math::Matrix<double, 3, 4> P1, P2;
P1(0, 0) = 0.919653; P1(0, 1) = -0.000621866; P1(0, 2) = -0.00124006; P1(0, 3) = 0.00255933;
P1(1, 0) = 0.000609954; P1(1, 1) = 0.919607; P1(1, 2) = -0.00957316; P1(1, 3) = 0.0540753;
P1(2, 0) = 0.00135482; P1(2, 1) = 0.0104087; P1(2, 2) = 0.999949; P1(2, 3) = -0.127624;
P2(0, 0) = 0.920039; P2(0, 1) = -0.0117214; P2(0, 2) = 0.0144298; P2(0, 3) = 0.0749395;
P2(1, 0) = 0.0118301; P2(1, 1) = 0.920129; P2(1, 2) = -0.00678373; P2(1, 3) = 0.862711;
P2(2, 0) = -0.0155846; P2(2, 1) = 0.00757181; P2(2, 2) = 0.999854; P2(2, 3) = -0.0887441;
/* 构造A矩阵 */
math::Matrix<double, 4, 4> A;
for (int i = 0; i<4; i++)
{
// p1
A(0, i) = p1[0] * P1(2, i) - P1(0, i);
A(1, i) = p1[1] * P1(2, i) - P1(1, i);
// p2
A(2, i) = p2[0] * P2(2, i) - P2(0, i);
A(3, i) = p2[1] * P2(2, i) - P2(1, i);
}
// SVD分解
math::Matrix<double, 4, 4> V;
math::matrix_svd<double, 4, 4>(A, nullptr, nullptr, &V);
math::Vec3f X;
X[0] = V(0, 3) / V(3, 3);
X[1] = V(1, 3) / V(3, 3);
X[2] = V(2, 3) / V(3, 3);
std::cout << " trianglede point is :" << X[0] << " " << X[1] << " " << X[2] << std::endl;
std::cout << " the result should be " << "2.14598 -0.250569 6.92321\n" << std::endl;
system("pause");
return 0;
}
OpenCV也对三角测量进行了封装(triangulatePoints()),原理大致相同,使用起来更加方便、简洁。
// 归一化,将像素座标转换到相机座标(非齐次座标)
Point2d pixel2cam(const Point2d& p, const Mat& K)
{
/* // 等价
Mat x = (Mat_<double>(3, 1) << p.x, p.y, 1);
x = K.inv()*x;
return Point2d(
x.at<double>(0,0),x.at<double>(1,0)
);
*/
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)
);
}
// 返回的是三维空间点
void triangulation(const vector<KeyPoint>& keypoint_1, const vector<KeyPoint>& keypoint_2, const vector<DMatch>& matches, const Mat& R, const Mat& t, vector<Point3d>& space_points)
{
// T1、T2为外参矩阵,视第一个相机座标系为世界座标系,则其R = I, T = 0
Mat T1 = (Mat_<double>(3, 4) <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0
);
Mat T2 = (Mat_<double>(3, 4) <<
R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
);
// 定义相机的内参矩阵
Mat K = (Mat_<double>(3, 3) <<
529.0, 0, 325.1,
0, 521.0, 249.9,
0, 0, 1
);
// 将所有匹配的特征点转化为归一化座标
vector<Point2d> pts_1, pts_2;
for (DMatch m : matches)
{
// 将像素座标转换至相机座标
pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
}
Mat pts_4d;
cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);
// 转换为非齐次座标
for (int i = 0; i < pts_4d.cols; i++)
{
Mat x = pts_4d.col(i); // 第i个三维点对应的齐次座标
x /= x.at<double>(3, 0);
space_points.push_back(Point3d(x.at<double>(0, 0), x.at<double>(1, 0), x.at<double>(2, 0)));
}
}
参考
- 《视觉SLAM14讲》-chapter7
- 深蓝学院《基于图像的三维模型重建》