已知2D-2D的匹配点,通过对极几何方法使用八点法求得本质矩阵,进而求解相机运动。特征点的3D位置可以在此之后通过三角化得到。或者由RGB-D相机确定。在使用双目或者RGB-D的视觉里程计中,可以直接使用3D到2D的方法。
for (DMatch m:matches) {
ushort d = d1.ptr<unsigned short>(int([m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
if (d == 0) // bad depth
continue;
float dd = d / 5000.0;
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
pts_2d.push_back(keypoints_2[m.trainIdx].pt);
}
cv的KeyPoint和DMatch数据类型
KeyPoint是一个类,用于表示在图像上的特征点。它包含以下属性:
- pt:关键点的坐标(x,y)
- size:关键点的尺度
- angle:关键点的方向
- response:关键点的响应值
- octave:关键点所在的金字塔层级
- class_id:可以用于区分不同的特征集的ID
DMatch是一个类,用于表示两幅图像之间的特征匹配。它包含以下属性:
- queryIdx:查询图像中关键点的索引
- trainIdx:训练图像中关键点的索引
- distance:两个特征点之间的距离
- imgIdx:当使用多个图像进行匹配时,用于标识匹配点属于哪个图像
这个for循环的意思是遍历matches中的所有匹配点(DMatch)。在每次循环中,变量m会依次代表matches中的一个匹配点。在循环体内,你可以根据具体需求进行匹配点的处理,比如根据距离筛选匹配点,计算匹配点的相似性得分等。
for (DMatch m : matches) {
// 处理每个匹配点
// 可以使用m.queryIdx和m.trainIdx分别获取查询图像和训练图像中的关键点索引
// 可以使用m.distance获取两个特征点之间的距离
// 其他操作...
}
keypoints_1[m.queryIdx]
:keypoints_1
是一个特征点(关键点)的向量,m.queryIdx
是一个匹配点的索引,这个语句的作用是通过索引获取对应的关键点。keypoints_1[m.queryIdx].pt
:pt
是KeyPoint
结构体中保存关键点坐标的成员变量,这个语句的作用是获取关键点的坐标keypoints_1[m.queryIdx].pt.y
:这个语句的作用是获取关键点的y坐标
d1.ptr<unsigned short>指向匹配点的像素坐标
这是将像素坐标转换成相机坐标
Point2d pixel2cam(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)
);
}
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K); //将匹配点的像素坐标。通过相机内参,转换到相机坐标系下
pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd)); //添加Z轴坐标
1、直接使用cv的solverPnP方法3D点与2D点的对应关系,求解相机的旋转和平移
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
cv::Rodrigues(r, R); // r为旋转向量形式,用Rodrigues公式转换为矩阵
2、BA高斯牛顿法
void bundleAdjustmentGaussNewton(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose) {
typedef Eigen::Matrix<double, 6, 1> Vector6d;
const int iterations = 10;
double cost = 0, lastCost = 0;
double fx = K.at<double>(0, 0);
double fy = K.at<double>(1, 1);
double cx = K.at<double>(0, 2);
double cy = K.at<double>(1, 2);
for (int iter = 0; iter < iterations; iter++) {
Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
Vector6d b = Vector6d::Zero();
cost = 0;
// compute cost
for (int i = 0; i < points_3d.size(); i++) {
Eigen::Vector3d pc = pose * points_3d[i];
double inv_z = 1.0 / pc[2];
double inv_z2 = inv_z * inv_z;
Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);
Eigen::Vector2d e = points_2d[i] - proj;
cost += e.squaredNorm();
Eigen::Matrix<double, 2, 6> J;
J << -fx * inv_z,
0,
fx * pc[0] * inv_z2,
fx * pc[0] * pc[1] * inv_z2,
-fx - fx * pc[0] * pc[0] * inv_z2,
fx * pc[1] * inv_z,
0,
-fy * inv_z,
fy * pc[1] * inv_z2,
fy + fy * pc[1] * pc[1] * inv_z2,
-fy * pc[0] * pc[1] * inv_z2,
-fy * pc[0] * inv_z;
H += J.transpose() * J;
b += -J.transpose() * e;
}
Vector6d dx;
dx = H.ldlt().solve(b);
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) {
// cost increase, update is not good
cout << "cost: " << cost << ", last cost: " << lastCost << endl;
break;
}
// update your estimation
pose = Sophus::SE3d::exp(dx) * pose;
lastCost = cost;
cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
if (dx.norm() < 1e-6) {
// converge
break;
}
}
cout << "pose by g-n: \n" << pose.matrix() << endl;
}
3、BAG2O方法
void bundleAdjustmentG2O(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose) {
// 构建图优化,先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType; // pose is 6, landmark is 3
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法,可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// vertex
VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
vertex_pose->setId(0);
vertex_pose->setEstimate(Sophus::SE3d());
optimizer.addVertex(vertex_pose);
// K
Eigen::Matrix3d K_eigen;
K_eigen <<
K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);
// edges
int index = 1;
for (size_t i = 0; i < points_2d.size(); ++i) {
auto p2d = points_2d[i];
auto p3d = points_3d[i];
EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
edge->setId(index);
edge->setVertex(0, vertex_pose);
edge->setMeasurement(p2d);
edge->setInformation(Eigen::Matrix2d::Identity());
optimizer.addEdge(edge);
index++;
}
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
pose = vertex_pose->estimate();
}