《视觉 SLAM 十四讲》第 7 讲 视觉里程计1 【如何根据图像 估计 相机运动】【特征点法】
github源码链接V2
文章目录
- 第 7 讲 视觉里程计1
-
- 7.1 特征点法
-
- 7.1.1 特征点
- 7.1.2 ORB 特征
-
- FAST 关键点 ⟹ \Longrightarrow ⟹ Oriented FAST
- BRIEF 描述子
- 7.1.3 特征匹配
- 7.2 实践 【Code】
-
-
-
- 本讲 CMakeLists.txt
-
- 7.2.1 使用 OpenCV 进行 ORB 的特征匹配 【Code】
- 7.2.2 手写 ORB 特征
-
- 估计 相机运动【相机位姿 估计】 3种情形 【对极几何、ICP、PnP】
- 7.3 2D-2D: 对极几何 单目相机(无距离信息)
-
- 7.3.2 本质矩阵 E \bm{E} E
- 7.3.3 单应矩阵(Homography)【墙、地面】
- 7.4 实践：对极约束 求解相机运动 【Code】
-
-
-
- 讨论！！！
-
-
- 7.5 三角测量
- 7.6 实践： 已知相机位姿，通过三角测量求特征点的空间位置 【Code】
-
- 7.6.2 三角测量的矛盾 ： 增加平移 Yes or No
- 7.7 3D-2D: PnP (Perspective-n-Point) 【最重要】
-
- 7.7.1 直接线性变换(DLT)
- 7.7.2 P3P 【3对点 估计位姿】
- 7.7.3 最小化 重投影误差 求解PnP
- 7.8 实践： 求解 PnP 【Code】
- 7.9 3D-3D: ICP(Iterative Closest Point, ICP，迭代最近点) 已知两个图的深度
-
- 7.9.1 SVD 方法
- 7.9.2 非线性优化方法
- 7.10 使用 SVD 及 非线性优化 来求解 ICP 【Code】
- 其它
-
-
- 查看opencv 版本命令
-
第 7 讲 视觉里程计1
图像特征点
在单幅 图像中 提取特征点 及 多幅图像 中 匹配特征点 的方法
对极几何 恢复图像之间 的 摄像机 的三维运动
PNP ICP
后续内容： 4 个模块 (视觉里程计、后端优化、回环检测、地图构建)
什么是特征点、如何提取和匹配特征点、如何根据配对的特征点估计相机运动。
——————————
7.1 特征点法
前端【视觉里程计】： 根据相邻图像的信息 估计 相机运动，给后端提供初值基础。
基于特征点法的前端： 对光照、动态物体不敏感。
视觉里程计 的两大类 算法： 特征点法 和 直接法。
- 如何提取、匹配图像特征点，然后估计两帧之间的相机运动和场景结构。【两帧间视觉里程计】
- 也称为
两视图几何 （Two-view geometry）
7.1.1 特征点
视觉里程计的核心： 如何根据图像 估计相机运动
有代表性的点
经典 SLAM 模型：路标
视觉SLAM：图像特征 ⟺ \iff ⟺ 路标
灰度值： 受光照、形变、物体材质影响严重，不稳定。
角点、边缘、区块
角点： 在不同图像之间 的 辨识度 更强。
2000年以前提出的特征：
更加稳定的局部图像特征：
可重复：相同的特征 可在不同图像中找到
可区别： 不同特征 表达不同
高效率： 同一图像，特征点的数量 远小于 像素数量。
本地性： 特征 仅与 一小片 图像区域相关。 【局部性？】
SIFT(尺度不变特征变换， Scale-Invariant Feature Transform)
计算量大
在一张图像中计算SIFT特征点 ⟺ \iff ⟺ 提取SIFT关键点， 并计算SIFT描述子。
关键点： 特征点的位置、朝向、大小等
描述子： 描述 该关键点 周围像素的信息。
两个特征点的描述子在向量空间上的距离相近 ⟺ \iff ⟺ 同样的特征点
ORB(Oriented FAST and Rotated BRIEF): 特征子具有旋转、尺度不变性，速度提升。
质量和性能之间的折中 成本、速度、匹配效果
7.1.2 ORB 特征
ORB贡献：
FAST角点提取 计算了 特征点的方向， 为后续 BRIEF描述子 增加了旋转不变性。
FAST 关键点 ⟹ \Longrightarrow ⟹ Oriented FAST
FAST 一种角点 检测 局部像素 灰度变化 明显的地方。 速度快
只比较 像素亮度 大小
预测试： 排除绝大多数不是角点 的像素。 加速 角点 检测
因为 一般要求 16个点里 N = 12 且连续， 因此 根据 这个间隔 4 要是超过两个点，就无法满足条件了。
避免 角点集中：在一定区域内 仅保留对应极大值的角点。非极大值抑制(Non-maximal suppression)
- Code 非极大值抑制 算法
优点：速度快【仅比较像素间亮度的差异】
不足：
1、重复性不强， 分布不均匀。
2、不具有 方向信息。 ⟹ \Longrightarrow ⟹ 灰度质心法(Intensity Centroid)
3、固定取半径为 3 的圆， 存在尺度问题 远看是角点，近看不是 ⟹ \Longrightarrow ⟹ 构建图像金字塔
FAST ⟹ \Longrightarrow ⟹ ORB 中的 Oriented FAST 【尺度+旋转】
质心： 以 图像块 灰度值 作为权重的中心
BRIEF 描述子
原始的BRIEF 描述子 不具有旋转不变性，在图像发生旋转时容易丢失。
7.1.3 特征匹配
特征匹配 数据关联 当前看到的路标与之前看到的路标之间的对应关系。
- 匹配 描述子
场景中 经常存在 大量重复纹理，特征描述相似 ⟶ \longrightarrow ⟶ 误匹配
两个二进制串之间的汉明距离—— 不同位数 的个数。
快速近似最近邻 (FLANN)
适用场景：匹配点数量极多
7.2 实践 【Code】
本讲 CMakeLists.txt
CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(vo1)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(OpenCV 4.2.0 REQUIRED)
find_package(G2O REQUIRED)
find_package(Sophus REQUIRED)
include_directories(
${OpenCV_INCLUDE_DIRS}
${G2O_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS}
"/usr/include/eigen3/"
)
add_executable(orb_cv orb_cv.cpp)
target_link_libraries(orb_cv ${OpenCV_LIBS})
#[[ # 块注释，用于选择 只运行 某个.cpp #[[]]
add_executable(orb_self orb_self.cpp)
target_link_libraries(orb_self ${OpenCV_LIBS})
# add_executable( pose_estimation_2d2d pose_estimation_2d2d.cpp extra.cpp ) # use this if in OpenCV2
add_executable(pose_estimation_2d2d pose_estimation_2d2d.cpp)
target_link_libraries(pose_estimation_2d2d ${OpenCV_LIBS})
# # add_executable( triangulation triangulation.cpp extra.cpp) # use this if in opencv2
add_executable(triangulation triangulation.cpp)
target_link_libraries(triangulation ${OpenCV_LIBS})
add_executable(pose_estimation_3d2d pose_estimation_3d2d.cpp)
target_link_libraries(pose_estimation_3d2d
g2o_core g2o_stuff
${OpenCV_LIBS}
${Sophus_LIBRARIES})
add_executable(pose_estimation_3d3d pose_estimation_3d3d.cpp)
target_link_libraries(pose_estimation_3d3d
g2o_core g2o_stuff
${OpenCV_LIBS}
${Sophus_LIBRARIES})
]]
7.2.1 使用 OpenCV 进行 ORB 的特征匹配 【Code】
报错：
/home/xixi/Downloads/slambook2-master/ch7/orb_cv.cpp:16:31: error: ‘CV_LOAD_IMAGE_COLOR’ was not declared in this scope
16 | Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
mkdir build && cd build
cmake ..
make
./orb_cv ../1.png ../2.png
orb_cv.cpp
#include 
去除误匹配： 汉明距离小于最小距离的 2 倍
7.2.2 手写 ORB 特征
改图片 路径
cd build
cmake ..
make
./orb_self
orb_self.cpp
//
// Created by xiang on 18-11-25.
//
#include 
估计 相机运动【相机位姿 估计】 3种情形 【对极几何、ICP、PnP】
1、相机为单目 ： 根据两组2D点 估计运动 对极几何
2、相机可获得距离信息(双目、RGB-D等)：两组3D点 估计运动 ICP
3、一组 3D + 一组 2D ： PnP
7.3 2D-2D: 对极几何 单目相机(无距离信息)
通过 二维图像点的对应关系， 恢复两帧之间摄像机的运动。
极平面(Epipolar plane)： O 1 , O 2 , P 三点形成的平面 O_1, O_2, P三点形成的平面 O1,O2,P三点形成的平面
- 注意 点 P P P 是 O 1 p 1 O_1p_1 O1p1 延长线 和 O 2 p 2 O_2p_2 O2p2 延长线 的交点
极点(Epipoles)： e 1 , e 2 e_1, e_2 e1,e2 【 O 1 O 2 O_1O_2 O1O2 连线 与 像平面 I 1 , I 2 I_1,I_2 I1,I2的交点】
极线(Epipolar line)： p 1 e 1 ( l 1 ) 、 p 2 e 2 ( l 2 ) p_1e_1(l_1)、p_2e_2(l_2) p1e1(l1)、p2e2(l2)
基线： O 1 O 2 O_1O_2 O1O2
像平面： I 1 ， I 2 I_1，I_2 I1，I2
假设 I 1 I_1 I1 中特征点 p 1 p_1 p1 匹配到 I 2 I_2 I2 中特征点 p 2 p_2 p2
本质矩阵(Essential Matrix) E = t ∧ R \bm{E} =\bm{t}^{\land}\bm{R} E=t∧R
基础矩阵(Fundamental Matrix) F = K − T E K − 1 \bm{F}=\bm{K}^{-T}\bm{E}\bm{K}^{-1} F=K−TEK−1
- E \bm{E} E 和 F \bm{F} F 只差了相机内参 K \bm{K} K 部分
对于归一化坐标 x 1 , x 2 \bm{x}_1, \bm{x}_2 x1,x2 ： x 2 T E x 1 = 0 \bm{x}_2^T\bm{E}\bm{x}_1=0 x2TEx1=0 【本质矩阵】
对于匹配的像素坐标 p 1 , p 2 \bm{p}_1, \bm{p}_2 p1,p2 ： p 2 T F p 1 = 0 \bm{p}_2^T\bm{F}\bm{p}_1=0 p2TFp1=0 【基础矩阵】
对极约束作用:
给出了两个匹配点的空间位置关系，将相机位姿估计问题变为以下两步：
1、根据配对点的像素位置 求出 E \bm{E} E 或 F \bm{F} F
2、根据 E \bm{E} E 或 F \bm{F} F 求出 R , t \bm{R,t} R,t
以 E \bm{E} E 为例，如何求解这两个问题
7.3.2 本质矩阵 E \bm{E} E
求解 E \bm{E} E：
根据已经估得的本质矩阵 E \bm{E} E， 恢复相机的运动 R , t \bm{R,t} R,t
7.3.3 单应矩阵(Homography)【墙、地面】
单应矩阵(Homography) H \bm{H} H：描述两个平面之间的映射关系。
-
运动估计 适用场景：场景中的特征点都落在同一平面上(墙、地面等)
-
无人机携带的俯视相机 或 扫地机携带的顶视相机
求解单应矩阵 H \bm{H} H:
直接线性变换法(Direct Linear Transform, DLT)
7.4 实践：对极约束 求解相机运动 【Code】
OpenCV官网相关API
报错：
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_2d2d.cpp:36:31: error: ‘CV_LOAD_IMAGE_COLOR’ was not declared in this scope
36 | Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
| ^~~~~~~~~~~~~~~~~~~
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_2d2d.cpp: In function ‘void pose_estimation_2d2d(std::vector<cv::KeyPoint>, std::vector<cv::KeyPoint>, std::vector<cv::DMatch>, cv::Mat&, cv::Mat&)’:
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_2d2d.cpp:143:61: error: ‘CV_FM_8POINT’ was not declared in this scope
143 | fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT);
| ^~~~~~~~~~~~
解决方案链接
之前 遇到了问题，改了CmakeLists.txt 很多地方，遇到了别的问题【Segmentation fault (core dumped)】，卡了挺久。重新复制原版CmakeLists.txt ，只改了OpenCV版本，CMAKE标准改成14。
cd build
cmake ..
make
./pose_estimation_2d2d ../1.png ../2.png
pose_estimation_2d2d.cpp
#include detector = FeatureDetector::create ( "ORB" );
// Ptr descriptor = DescriptorExtractor::create ( "ORB" );
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> match;
//BFMatcher matcher ( NORM_HAMMING );
matcher->match(descriptors_1, descriptors_2, match);
//-- 第四步:匹配点对筛选
double min_dist = 10000, max_dist = 0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_dist);
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
for (int i = 0; i < descriptors_1.rows; i++) {
if (match[i].distance <= max(2 * min_dist, 30.0)) {
matches.push_back(match[i]);
}
}
}
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)
);
}
void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
std::vector<KeyPoint> keypoints_2,
std::vector<DMatch> matches,
Mat &R, Mat &t) {
// 相机内参,TUM Freiburg2
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
//-- 把匹配点转换为vector的形式
vector<Point2f> points1;
vector<Point2f> points2;
for (int i = 0; i < (int) matches.size(); i++) {
points1.push_back(keypoints_1[matches[i].queryIdx].pt);
points2.push_back(keypoints_2[matches[i].trainIdx].pt);
}
//-- 计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat(points1, points2, FM_8POINT); // OpenCV4 修改
cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;
//-- 计算本质矩阵
Point2d principal_point(325.1, 249.7); //相机光心, TUM dataset标定值
double focal_length = 521; //相机焦距, TUM dataset标定值
Mat essential_matrix;
essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
cout << "essential_matrix is " << endl << essential_matrix << endl;
//-- 计算单应矩阵
//-- 但是本例中场景不是平面，单应矩阵意义不大
Mat homography_matrix;
homography_matrix = findHomography(points1, points2, RANSAC, 3);
cout << "homography_matrix is " << endl << homography_matrix << endl;
//-- 从本质矩阵中恢复旋转和平移信息.
// 此函数仅在Opencv3中提供
recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
cout << "R is " << endl << R << endl;
cout << "t is " << endl << t << endl;
}
讨论！！！
7.5 三角测量
在单目 SLAM 中，仅通过 单张图像 无法获得像素的深度信息，需要通过三角测量(Triangulation)（或三角化） 估计地图点的深度
三角测量： 通过不同位置对同一路标点进行观察，从观察到的位置判断路标点的距离。
- 通过不同季节观察到的星星的角度，估计它与我们的距离。
7.6 实践： 已知相机位姿，通过三角测量求特征点的空间位置 【Code】
cd build
cmake ..
make
./triangulation ../1.png ../2.png
triangulation.cpp
#include detector = FeatureDetector::create ( "ORB" );
// Ptr descriptor = DescriptorExtractor::create ( "ORB" );
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> match;
// BFMatcher matcher ( NORM_HAMMING );
matcher->match(descriptors_1, descriptors_2, match);
//-- 第四步:匹配点对筛选
double min_dist = 10000, max_dist = 0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_dist);
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
for (int i = 0; i < descriptors_1.rows; i++) {
if (match[i].distance <= max(2 * min_dist, 30.0)) {
matches.push_back(match[i]);
}
}
}
void pose_estimation_2d2d(
const std::vector<KeyPoint> &keypoints_1,
const std::vector<KeyPoint> &keypoints_2,
const std::vector<DMatch> &matches,
Mat &R, Mat &t) {
// 相机内参,TUM Freiburg2
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
//-- 把匹配点转换为vector的形式
vector<Point2f> points1;
vector<Point2f> points2;
for (int i = 0; i < (int) matches.size(); i++) {
points1.push_back(keypoints_1[matches[i].queryIdx].pt);
points2.push_back(keypoints_2[matches[i].trainIdx].pt);
}
//-- 计算本质矩阵
Point2d principal_point(325.1, 249.7); //相机主点, TUM dataset标定值
int focal_length = 521; //相机焦距, TUM dataset标定值
Mat essential_matrix;
essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
//-- 从本质矩阵中恢复旋转和平移信息.
recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
}
void triangulation(
const vector<KeyPoint> &keypoint_1,
const vector<KeyPoint> &keypoint_2,
const std::vector<DMatch> &matches,
const Mat &R, const Mat &t,
vector<Point3d> &points) {
Mat T1 = (Mat_<float>(3, 4) <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0);
Mat T2 = (Mat_<float>(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) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
vector<Point2f> 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);
x /= x.at<float>(3, 0); // 归一化
Point3d p(
x.at<float>(0, 0),
x.at<float>(1, 0),
x.at<float>(2, 0)
);
points.push_back(p);
}
}
Point2f pixel2cam(const Point2d &p, const Mat &K) {
return Point2f
(
(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
);
}
7.6.2 三角测量的矛盾 ： 增加平移 Yes or No
1、平移很小时， 像素上的不确定性 将导致 较大的深度不确定性。
- 特征点 运动一个 像素 Δ x \Delta x Δx ⟶ \longrightarrow ⟶ 视线角 变换一个角度 Δ θ \Delta \theta Δθ ⟶ \longrightarrow ⟶ 将测量到 深度值变化 Δ d \Delta d Δd
- 当 t \bm{t} t 较大时， Δ d \Delta d Δd 将明显变小。说明平移较大时，在同样的相机分辨率下，三角化测量将会更精确。
提高三角化精度的 2 种方法：
1、提高特征点的提取精度，也就是提高图像分辨率 ⟶ \longrightarrow ⟶ 图像变大，增加计算成本
2、增大平移量 ⟶ \longrightarrow ⟶ 图像外观发生明显变化，使得特征提取与匹配变得困难
三角化的矛盾 【视差(parallax)】: 增大平移，可能导致匹配失效；而平移太小，则三角化精度不够。
——————————
2D-2D 的 对极几何法 的 不足
1、需要8个或8个以上的点对
2、存在初始化、纯旋转和尺度的问题
7.7 3D-2D: PnP (Perspective-n-Point) 【最重要】
当知道 n 个 3D 空间点及其投影位置时，如何估计相机的位姿。
如果两张图像中的一张特征点的 3D 位置已知，最少需要 3 个点对(以及至少一个额外点验证结果) 即可估计相机运动。
特征点的3D位置获取方法： 三角化 或 RGB-D相机的深度图
双目/RGB-D
单目
视觉里程计
PnP估计相机运动
需先初始化
3D-2D 方法的优点：
不需要使用对极约束，又可以在很少的匹配点获得较好的运动估计。
7.7.1 直接线性变换(DLT)
适用场景：
1、已知一组3D点的位置，以及它们在某个相机中的投影位置，求该相机的位姿。
2、给定地图和图像，求解相机状态。
3、把 3D 点看成在另一个相机坐标系中的点， 用来求解两个相机的相对运动。
7.7.2 P3P 【3对点 估计位姿】
P3P 不足：
1、只用了 3个点的信息，浪费了其它信息
2、如果3D 点 或 2D 点 受噪声影响，或存在 误匹配 ，则算法失效。
——> EPnP、UPnP
- 利用更多的信息，用迭代的方式对相机位姿进行优化，尽可能消除噪声的影响。
SLAM中的通常做法： 先使用 P3P/EPnP 等方法估计相机位姿，再构建最小二乘优化问题对估计值进行调整。
7.7.3 最小化 重投影误差 求解PnP
线性方法： 先求相机位姿，再求空间点位置
非线性优化： 把相机和三维点放在一起优化 【Bundle Adjustment】
3D 点的投影位置 与 观测位置 作差 【重投影误差】
优化特征点的空间位置：
7.8 实践： 求解 PnP 【Code】
7.8.1 使用 PnP 求解位姿
要修改的报错：
报错1：
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:37:11: error: ‘Sophus::SE3d’ has not been declared
37 | Sophus::SE3d &pose
| ^~~~
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:45:11: error: ‘Sophus::SE3d’ has not been declared
45 | Sophus::SE3d &pose
代码里所有的 SE3d 去掉d
报错2：
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:54:31: error: ‘CV_LOAD_IMAGE_COLOR’ was not declared in this scope
54 | Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
| ^~~~~~~~~~~~~~~~~~~
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:64:28: error: ‘CV_LOAD_IMAGE_UNCHANGED’ was not declared in this scope
64 | Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED); // 深度图为16位无符号数，单通道图像
opencv3 opencv4
CV_LOAD_IMAGE_UNCHANGED IMREAD_UNCHANGED
CV_LOAD_IMAGE_GRAYSCALE IMREAD_GRAYSCALE
CV_LOAD_IMAGE_COLOR IMREAD_COLOR
CV_LOAD_IMAGE_ANYDEPTH IMREAD_ANYDEPTH
报错3：
/usr/local/include/g2o/stuff/tuple_tools.h:41:46: error: ‘tuple_size_v’ is not a member of ‘std’; did you mean ‘tuple_size’?
41 | f, t, i, std::make_index_sequence<std::tuple_size_v<std::decay_t<T>>>());
解决办法：
在 CMakeLists.txt 中添加 set(CMAKE_CXX_STANDARD 17)
报错4：
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d2d.cpp:318:10: error: ‘make_unique’ is not a member of ‘g2o’; did you mean ‘std::make_unique’?
318 | g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
类似第6讲，直接替换 代码块
// 构建图优化，先设定g2o typedef 别名替换
/*typedef g2o::BlockSolver> BlockSolverType; // 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型
*/
std::unique_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver
(new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>());
// 梯度下降方法，可以从GN, LM, DogLeg 中选
/*auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique(g2o::make_unique()));*/
std::unique_ptr<g2o::BlockSolverX> solver_ptr (new g2o::BlockSolverX(std::move(linearSolver)));
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
报错5：
/usr/bin/ld: CMakeFiles/pose_estimation_3d2d.dir/pose_estimation_3d2d.cpp.o: in function `bundleAdjustmentGaussNewton(std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, std::vector<Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 2, 1, 0, 2, 1> > > const&, cv::Mat const&, Sophus::SE3&)':
pose_estimation_3d2d.cpp:(.text+0x2a4f): undefined reference to `Sophus::SE3::operator*(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&) const'
/usr/bin/ld: pose_estimation_3d2d.cpp:(.text+0x3254): undefined reference to `Sophus::SE3::exp(Eigen::Matrix const&)'
是CMakeLists.txt 里没链接到 Sophus，加上即可
add_executable(pose_estimation_3d2d pose_estimation_3d2d.cpp)
target_link_libraries(pose_estimation_3d2d
g2o_core g2o_stuff
${OpenCV_LIBS}
${Sophus_LIBRARIES})
cd build
cmake ..
make
./pose_estimation_3d2d ../1.png ../2.png ../1_depth.png ../2_depth.png
pose_estimation_3d2d.cpp
#include detector = FeatureDetector::create ( "ORB" );
// Ptr descriptor = DescriptorExtractor::create ( "ORB" );
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> match;
// BFMatcher matcher ( NORM_HAMMING );
matcher->match(descriptors_1, descriptors_2, match);
//-- 第四步:匹配点对筛选
double min_dist = 10000, max_dist = 0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_dist);
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
for (int i = 0; i < descriptors_1.rows; i++) {
if (match[i].distance <= max(2 * min_dist, 30.0)) {
matches.push_back(match[i]);
}
}
}
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)
);
}
void bundleAdjustmentGaussNewton(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3 &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::SE3::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;
}
/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void setToOriginImpl() override {
_estimate = Sophus::SE3();
}
/// left multiplication on SE3
virtual void oplusImpl(const double *update) override {
Eigen::Matrix<double, 6, 1> update_eigen;
update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
_estimate = Sophus::SE3::exp(update_eigen) * _estimate;
}
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
};
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
virtual void computeError() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
Sophus::SE3 T = v->estimate();
Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
pos_pixel /= pos_pixel[2];
_error = _measurement - pos_pixel.head<2>();
}
virtual void linearizeOplus() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
Sophus::SE3 T = v->estimate();
Eigen::Vector3d pos_cam = T * _pos3d;
double fx = _K(0, 0);
double fy = _K(1, 1);
double cx = _K(0, 2);
double cy = _K(1, 2);
double X = pos_cam[0];
double Y = pos_cam[1];
double Z = pos_cam[2];
double Z2 = Z * Z;
_jacobianOplusXi
<< -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
}
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
private:
Eigen::Vector3d _pos3d;
Eigen::Matrix3d _K;
};
void bundleAdjustmentG2O(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3 &pose) {
// 构建图优化，先设定g2o typedef 别名替换
/*typedef g2o::BlockSolver> BlockSolverType; // 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型
*/
std::unique_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver
(new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>());
// 梯度下降方法，可以从GN, LM, DogLeg 中选
/*auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique(g2o::make_unique()));*/
std::unique_ptr<g2o::BlockSolverX> solver_ptr (new g2o::BlockSolverX(std::move(linearSolver)));
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));
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::SE3());
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();
}
7.8.3 使用 g2o 进行 BA 优化
7.9 3D-3D: ICP(Iterative Closest Point, ICP，迭代最近点) 已知两个图的深度
迭代最近点： 认为距离最近的两个点为同一个。
7.9.1 SVD 方法
7.9.2 非线性优化方法
7.10 使用 SVD 及 非线性优化 来求解 ICP 【Code】
7.10.1 SVD方法
通过特征匹配 获取两组 3D 点，最后用 ICP 计算 位姿变换
7.10.2 非线性优化方法
根据 7.8 节改一遍
新的报错：
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d3d.cpp:81:50: error: ‘Sophus::SO3d’ has not been declared
81 | _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
去掉d，改成 SO3
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d3d.cpp: In function ‘void bundleAdjustment(const std::vector<cv::Point3_<float> >&, const std::vector<cv::Point3_<float> >&, cv::Mat&, cv::Mat&)’:
/home/xixi/Downloads/slambook2-master/ch7/pose_estimation_3d3d.cpp:298:41: error: ‘const EstimateType’ {aka ‘const class Sophus::SE3’} has no member named ‘rotationMatrix’; did you mean ‘rotation_matrix’?
298 | Eigen::Matrix3d R_ = pose->estimate().rotationMatrix();
| ^~~~~~~~~~~~~~
| rotation_matrix
按照提示改成
Eigen::Matrix3d R_ = pose->estimate().rotation_matrix();
cd build
cmake ..
make
./pose_estimation_3d3d ../1.png ../2.png ../1_depth.png ../2_depth.png
pose_estimation_3d3d.cpp
#include detector = FeatureDetector::create ( "ORB" );
// Ptr descriptor = DescriptorExtractor::create ( "ORB" );
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> match;
// BFMatcher matcher ( NORM_HAMMING );
matcher->match(descriptors_1, descriptors_2, match);
//-- 第四步:匹配点对筛选
double min_dist = 10000, max_dist = 0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_dist);
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
for (int i = 0; i < descriptors_1.rows; i++) {
if (match[i].distance <= max(2 * min_dist, 30.0)) {
matches.push_back(match[i]);
}
}
}
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)
);
}
void pose_estimation_3d3d(const vector<Point3f> &pts1,
const vector<Point3f> &pts2,
Mat &R, Mat &t) {
Point3f p1, p2; // center of mass
int N = pts1.size();
for (int i = 0; i < N; i++) {
p1 += pts1[i];
p2 += pts2[i];
}
p1 = Point3f(Vec3f(p1) / N);
p2 = Point3f(Vec3f(p2) / N);
vector<Point3f> q1(N), q2(N); // remove the center
for (int i = 0; i < N; i++) {
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}
// compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for (int i = 0; i < N; i++) {
W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
}
cout << "W=" << W << endl;
// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
cout << "U=" << U << endl;
cout << "V=" << V << endl;
Eigen::Matrix3d R_ = U * (V.transpose());
if (R_.determinant() < 0) {
R_ = -R_;
}
Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);
// convert to cv::Mat
R = (Mat_<double>(3, 3) <<
R_(0, 0), R_(0, 1), R_(0, 2),
R_(1, 0), R_(1, 1), R_(1, 2),
R_(2, 0), R_(2, 1), R_(2, 2)
);
t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}
void bundleAdjustment(
const vector<Point3f> &pts1,
const vector<Point3f> &pts2,
Mat &R, Mat &t) {
// 构建图优化，先设定g2o typedef 别名替换
/*typedef g2o::BlockSolver> BlockSolverType; // 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型
*/
std::unique_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver
(new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>());
// 梯度下降方法，可以从GN, LM, DogLeg 中选
/*auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique(g2o::make_unique()));*/
std::unique_ptr<g2o::BlockSolverX> solver_ptr (new g2o::BlockSolverX(std::move(linearSolver)));
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// vertex
VertexPose *pose = new VertexPose(); // camera pose
pose->setId(0);
pose->setEstimate(Sophus::SE3());
optimizer.addVertex(pose);
// edges
for (size_t i = 0; i < pts1.size(); i++) {
EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(
Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
edge->setVertex(0, pose);
edge->setMeasurement(Eigen::Vector3d(
pts1[i].x, pts1[i].y, pts1[i].z));
edge->setInformation(Eigen::Matrix3d::Identity());
optimizer.addEdge(edge);
}
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
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 << endl << "after optimization:" << endl;
cout << "T=\n" << pose->estimate().matrix() << endl;
// convert to cv::Mat
Eigen::Matrix3d R_ = pose->estimate().rotation_matrix();
Eigen::Vector3d t_ = pose->estimate().translation();
R = (Mat_<double>(3, 3) <<
R_(0, 0), R_(0, 1), R_(0, 2),
R_(1, 0), R_(1, 1), R_(1, 2),
R_(2, 0), R_(2, 1), R_(2, 2)
);
t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}
使用了越来越多的信息：
| 对极几何 | PnP | ICP |
|---|---|---|
| 没有深度 | 一个图的深度 | 两个图的深度 |
7.11 小结
其它
查看opencv 版本命令
sudo apt update
sudo apt install libopencv-dev python3-opencv
python3 -c "import cv2; print(cv2.__version__)"
sophus安装
Ubuntu20.04安装Ceres和g2o
