视觉slam学习笔记以及课后习题《第五讲特征点法视觉里程计》
这篇博客主要记录了我在深蓝学院视觉slam课程中的课后习题，因为是为了统计知识点来方便自己以后查阅，所以有部分知识可能不太严谨，如果给大家造成了困扰请见谅，大家发现了问题也可以私信或者评论给我及时改正，欢迎大家一起探讨。其他章的笔记可以在我的首页文章中查看。
整个作业的代码和文档都可以参考我的GitHub存储库GitHub - 1481588643/vslam
如果想要了解视觉slam其他章节的内容，也可以查阅以下链接视觉slam学习笔记以及课后习题《第三讲李群李代数》_m0_61238051的博客-CSDN博客
视觉slam学习笔记以及课后习题《第一讲初识slam》_m0_61238051的博客-CSDN博客_slam下载地址
视觉slam学习笔记以及课后习题《第二讲三维物体刚体运动》_m0_61238051的博客-CSDN博客
视觉slam学习笔记以及课后习题《第四讲相机模型和非线性优化》_m0_61238051的博客-CSDN博客
一.ORB 特征点 (4 分，约 3 小时)
ORB(Oriented FAST and BRIEF) 特征是 SLAM 中一种很常用的特征，由于其二进制特性，使得它可以非常快速地提取与计算 [1]。下面，你将按照本题的指导，自行书写 ORB 的提取、描述子的计算以及匹配的代码。代码框架参照 computeORB.cpp 文件，图像见 1.png 文件和 2.png。
1.ORB 提 取
ORB 即 Oriented FAST 简称。它实际上是 FAST 特征再加上一个旋转量。本习题将使用 OpenCV 自带的 FAST 提取算法，但是你要完成旋转部分的计算。旋转的计算过程描述如下 [2]：
实际上只需计算 m01 和 m10 即可。习题中取图像块大小为 16x16，即对于任意点 (u, v)，图像块从
(u − 8, v − 8) 取到 (u + 7, v + 7) 即可。请在习题的 computeAngle 中，为所有特征点计算这个旋转角。提示：
- 由于要取图像 16x16 块，所以位于边缘处的点（比如 u < 8 的）对应的图像块可能会出界，此时需要判断该点是否在边缘处，并跳过这些点。
- 由于矩的定义方式，在画图特征点之后，角度看起来总是指向图像中更亮的地方。
- std::atan 和 std::atan2 会返回弧度制的旋转角，但 OpenCV 中使用角度制，如使用 std::atan 类函数，请转换一下。
作为验证，第一个图像的特征点如图 1 所示。看不清可以放大看。
// compute the angle
void computeAngle(const cv::Mat &image, vector &keypoints) {
int half_patch_size = 8;
for (auto &kp : keypoints) {
// START YOUR CODE HERE (~7 lines)
//judge if keypoint is on edge
int x=cvRound(kp.pt.x);
int y=cvRound(kp.pt.y);
if( x-half_patch_size<0||x+half_patch_size>image.cols||
y-half_patch_size<0||y+half_patch_size>image.rows)
continue; //结束当前循环，进入到下一次循环
double m01=0,m10=0; //定义变量的时候，要初始化，不然这里第一张图片所有kp.angle=0
for(int i=-half_patch_size;i(y+j,x+i); //真实坐标(j,i)+(y,x)
m10 += i*image.at(y+j,x+i); //获得单个像素值image.at(y,x)
}
}
kp.angle = atan(m01/m10)*180/pi;
cout<<"m10 = "<
2.ORB 描 述
ORB 描述即带旋转的 BRIEF 描述。所谓 BRIEF 描述是指一个 0-1 组成的字符串（可以取 256 位或
128 位），每一个 bit 表示一次像素间的比较。算法流程如下：
这样我们就得到了 ORB 的描述。我们在程序中用 256 个 bool 变量表达这个描述2。请你完成 compute- ORBDesc 函数，实现此处计算。注意，通常我们会固定 p, q 的取法（称为 ORB 的 pattern），否则每次都重新随机选取，会使得描述不稳定。我们在全局变量 ORB_pattern 中定义了 p, q 的取法，格式为up, vp, uq, vq。请你根据给定的 pattern 完成 ORB 描述的计算。
提示：
- p, q 同样要做边界检查，否则会跑出图像外。如果跑出图像外，就设这个描述子为空。
- 调用 cos 和 sin 时同样请注意弧度和角度的转换。
void computeORBDesc(const cv::Mat &image, vector &keypoints,
vector &desc) {
for (auto &kp : keypoints) {
DescType d(256, false);
for (int i = 0; i < 256; i++) {
// START YOUR CODE HERE (~7 lines)
auto cos_ = float(cos(kp.angle*pi/180)); //将角度转换成弧度再进行cos、sin的计算
auto sin_ = float(sin(kp.angle*pi/180));
//注意pattern中的数如何取
cv::Point2f p_r(cos_*ORB_pattern[4*i]-sin_*ORB_pattern[4*i+1],
sin_*ORB_pattern[4*i]+cos_*ORB_pattern[4*i+1]);
cv::Point2f q_r(cos_*ORB_pattern[4*i+2]-sin_*ORB_pattern[4*i+3],
sin_*ORB_pattern[4*i+2]+cos_*ORB_pattern[4*i+3]);
cv::Point2f p(kp.pt+p_r); //获取p'与q'的真实坐标，才能获得其像素值
cv::Point2f q(kp.pt+q_r);
// if kp goes outside, set d.clear()
if(p.x<0||p.y<0||p.x>image.cols||p.y>image.rows||
q.x<0||q.y<0||q.x>image.cols||q.y>image.rows){
d.clear();
break;
}
//像素值比较
d[i]=image.at(p)>image.at(q)?0:1;
// END YOUR CODE HERE
}
desc.push_back(d);
}
int bad = 0;
for (auto &d : desc) {
if (d.empty())
bad++;
}
cout << "bad/total: " << bad << "/" << desc.size() << endl;
return;
}
3.暴力匹配
在提取描述之后，我们需要根据描述子进行匹配。暴力匹配是一种简单粗暴的匹配方法，在特征点不 多时很有用。下面你将根据习题指导，书写暴力匹配算法。
所谓暴力匹配思路很简单。给定两组描述子 P = [p1, . . . , pM ] 和 Q = [q1, . . . , qN ]。那么，对 P 中任意一个点，找到 Q 中对应最小距离点，即算一次匹配。但是这样做会对每个特征点都找到一个匹配，所以我们通常还会限制一个距离阈值 dmax，即认作匹配的特征点距离不应该大于 dmax。下面请你根据上述描述，实现函数 bfMatch，返回给定特征点的匹配情况。实践中取 dmax = 50。
1注意反过来记也可以，但是程序中要保持一致。
2严格来说可以用 32 个 uchar 以节省空间，但是那样涉及到位运算，本习题只要求掌握算法。
// brute-force matching
void bfMatch(const vector &desc1, const vector &desc2,
vector &matches) {
int d_max = 50;
// START YOUR CODE HERE (~12 lines)
// find matches between desc1 and desc2.
for(int i=0;i 4.多线程 ORB
C++17 标准中带来了很多语言层面的并行化支持。它们对算法开发人员非常友好，我们可以很轻松地借助标准库的内容，就写出稳定、可靠的并行程序。在 ORB 这个例子中，很明显，角点方向的计算和描述子的计算都是很容易并行化的。请根据你在前两题中的结果，实现多线程并行化的 ORB 描述子计算过程，并比较多线程与单线程之间的性能差异。
为了方便起见，我为你搭建了计算角点部分的多线程计算方法框架，你只需填入关键代码部分即可。而 对于计算描述子部分，请参考角度计算部分来完成。如果你的编译器还不支持 17 标准，请升级你的编译器。
提示：
1.你需要按位计算两个描述子之间的汉明距离。
2.作为验证，匹配之后输出图像应如图 2 所示。
3.OpenCV 的 DMatch 结构，queryIdx 为第一图的特征 ID，trainIdx 为第二个图的特征 ID。最后，请结合实验，回答下面几个问题：
1.为什么说 ORB 是一种二进制特征？
答：1.因为ORB的描述子采用的是0和1表示。
2.为什么在匹配时使用 50 作为阈值，取更大或更小值会怎么样？
答：50是经验值，阈值变大匹配到的点数增加，匹配错误会增加，阈值变小匹配点数减少
3.暴力匹配在你的机器上表现如何？你能想到什么减少计算量的匹配方法吗？
答：2.7秒，FLANN可以减少时间。
4.多线程版本相比单线程版本是否有提升？在你的机器上大约能提升多少性能？
void computeAngleMT(const cv::Mat &image, vector &keypoints) {
int half_patch_size = 8;
std::for_each(std::execution::par_unseq, keypoints.begin(), keypoints.end(),
[&half_patch_size, &image](auto &kp) {
// START YOUR CODE HERE
int x=cvRound(kp.pt.x);
int y=cvRound(kp.pt.y);
if( x-half_patch_size>=0&&x+half_patch_size=0&&y+half_patch_size<=image.rows){
// continue; //结束当前循环，进入到下一次循环
double m01=0,m10=0; //定义变量的时候，要初始化，不然这里第一张图片所有kp.angle=0
for(int i=-half_patch_size;i(y+j,x+i); //真实坐标(j,i)+(y,x)
m10 += i*image.at(y+j,x+i); //获得单个像素值image.at(y,x)
}
}
kp.angle = atan(m01/m10)*180/pi;
cout<<"m10 = "<
二.从 E 恢 复 R, t (3 分，约 1 小时)
#include
#include
#include
using namespace Eigen;
#include
#include
using namespace std;
int main(int argc, char **argv) {
// 给定Essential矩阵
Matrix3d E;
E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
-0.006788487241438284, -0.5815434272915686, -0.01438258684486258;
// 待计算的R,t
Matrix3d R;
Vector3d t;
// SVD and fix sigular values
// START YOUR CODE HERE
JacobiSVD svd(E,ComputeThinU | ComputeThinV);
Matrix3d U=svd.matrixU();
Matrix3d V=svd.matrixV();
VectorXd sigma_value=svd.singularValues();
Matrix3d SIGMA=U.inverse()*E*V.transpose().inverse();
Vector3d sigma_value2={(sigma_value[0]+sigma_value[1])/2,(sigma_value[0]+sigma_value[1])/2,0};
Matrix3d SIGMA2=sigma_value2.asDiagonal();
cout<<"SIGMA=\n"<
三.用 G-N 实现 Bundle Adjustment 中的位姿估计 (3 分 ， 约 2 小时)
答：
1.
2.
3.左乘exp（dx），扰动模型
#include
#include
using namespace Eigen;
#include
#include
#include
#include
#include "sophus/se3.hpp"
using namespace std;
typedef vector> VecVector3d;
typedef vector> VecVector2d;
typedef Matrix Vector6d;
string p3d_file = "./p3d.txt";
string p2d_file = "./p2d.txt";
int main(int argc, char **argv) {
VecVector2d p2d;
VecVector3d p3d;
Matrix3d K;
double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
K << fx, 0, cx, 0, fy, cy, 0, 0, 1;
// load points in to p3d and p2d
// START YOUR CODE HERE
ifstream fin(p3d_file);
if(!fin)
{
cout<<"不能打开文件"<>x>>y>>z;
Vector3d v(x,y,z);
p3d.push_back(v);
}
ifstream fins(p2d_file);
if(!fins)
{
cout<<"不能打开文件"<>x>>y;
Vector2d v(x,y);
p2d.push_back(v);
}
// END YOUR CODE HERE
assert(p3d.size() == p2d.size());
int iterations = 100;
double cost = 0, lastCost = 0;
int nPoints = p3d.size();
cout << "points: " << nPoints << endl;
Sophus::SE3d T_esti; // estimated pose
for (int iter = 0; iter < iterations; iter++) {
Matrix H = Matrix::Zero();
Vector6d b = Vector6d::Zero();
cost = 0;
// compute cost
for (int i = 0; i < nPoints; i++) {
// compute cost for p3d[I] and p2d[I]
// STA double X=p3d[i][0];
double X=p3d[i][0] ;
double Y=p3d[i][1];
double Z=p3d[i][2];
Vector4d P_0(X,Y,Z,1);
Vector4d P=T_esti.matrix()*P_0;
Vector3d u=K*Vector3d(P(0,0),P(1,0),P(2,0));
Vector2d e=p2d[i]-Vector2d(u(0,0)/u(2,0),u(1,0)/u(2,0));
cost+=e.squaredNorm()/2;
// END YOUR CODE HERE
// compute jacobian
Matrix J;
// START YOUR CODE HERE
J(0,0)=fx/Z;
J(0,1)=0;
J(0,2)=-fx*X/(Z*Z);
J(0,3)=-fx*X*Y/(Z*Z);
J(0,4)=fx+fx*X*X/(Z*Z);
J(0,5)=-fx*Y/Z;
J(1,0)=0;
J(1,1)=fy/Z;
J(1,2)=-fy*Y/(Z*Z);
J(1,3)=-fy-fy*Y*Y/(Z*Z);
J(1,4)=fy*X*Y/(Z*Z);
J(1,5)=fy*X/Z;
J=-J;
// END YOUR CODE HERE
H += J.transpose() * J;
b += -J.transpose() * e;
}
// solve dx
Vector6d dx;
// START YOUR CODE HERE
dx=H.ldlt().solve(b);
// END YOUR CODE HERE
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
// START YOUR CODE HERE
T_esti=Sophus::SE3d::exp(dx)*T_esti;
// END YOUR CODE HERE
lastCost = cost;
cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
}
cout << "estimated pose: \n" << T_esti.matrix() << endl;
return 0;
}
但是书中由于代码中错误地设置了 depth scale（应该为 5000，实际输入了 1000），所以应该说和修正后结果相近。
四.* 用 ICP 实现轨迹对齐 (2 分，约 2 小时)
在实际当中，我们经常需要比较两条轨迹之间的误差。第三节课习题中，你已经完成了两条轨迹之间 的 RMSE 误差计算。但是，由于 ground-truth 轨迹与相机轨迹很可能不在一个参考系中，它们得到的轨迹并不能直接比较。这时，我们可以用 ICP 来计算两条轨迹之间的相对旋转与平移，从而估计出两个参考系之间的差异。
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
using namespace Eigen;
using namespace cv;
void ReadData(string FileName ,
vector> &poses_e,
vector> &poses_g,
vector &t1,
vector &t2
);
void icp_svd (
const vector& pts1,
const vector& pts2,
Matrix3d & R,Vector3d& t);
void DrawTrajectory(vector> poses_e,
vector> poses_g);
int main(int argc,char **argv){
string TrajectoryFile = "./compare.txt";
vector> poses_e;
vector> poses_g;
vector> poses_g_; //poses_g_=T*poses_g
Eigen::Matrix3d R;
Eigen::Vector3d t;
vector t_e,t_g;
ReadData( TrajectoryFile,poses_e, poses_g, t_e, t_g);
icp_svd(t_e,t_g,R,t);
Sophus::SE3 T_eg(R,t);
for(auto SE3_g:poses_g){
SE3_g =T_eg*SE3_g; // T_e[i]=T_eg*T_g[i]
poses_g_.push_back(SE3_g);
}
DrawTrajectory(poses_e,poses_g_);
}
/*************读取文件中的位姿******************/
void ReadData(string FileName ,
vector> &poses_e,
vector> &poses_g,
vector &t_e,
vector &t_g
){
string line;
double time1,tx_1,ty_1,tz_1,qx_1,qy_1,qz_1,qw_1;
double time2,tx_2,ty_2,tz_2,qx_2,qy_2,qz_2,qw_2;
ifstream fin(FileName);
if(!fin.is_open()){
cout<<"compare.txt file can not open!"<>time1 >> tx_1 >> ty_1 >> tz_1 >> qx_1 >> qy_1 >> qz_1 >> qw_1
>>time2 >> tx_2 >> ty_2 >> tz_2 >> qx_2 >> qy_2 >> qz_2 >> qw_2;
t_e.push_back(Point3d(tx_1,ty_1,tz_1)); //将t取出，为了进行用icp进行计算
t_g.push_back(Point3d(tx_2,ty_2,tz_2));
Eigen::Vector3d point_t1(tx_1, ty_1, tz_1);
Eigen::Vector3d point_t2(tx_2, ty_2, tz_2);
Eigen::Quaterniond q1 = Eigen::Quaterniond(qw_1, qx_1, qy_1, qz_1).normalized(); //四元数的顺序要注意
Eigen::Quaterniond q2 = Eigen::Quaterniond(qw_2, qx_2, qy_2, qz_2).normalized();
Sophus::SE3 SE3_qt1(q1, point_t1);
Sophus::SE3 SE3_qt2(q2, point_t2);
poses_e.push_back(SE3_qt1);
poses_g.push_back(SE3_qt2);
}
}
void icp_svd (
const vector& pts1,
const vector& pts2,
Matrix3d& R, Vector3d& t) {
Point3f p1, p2; // center of mass
int N = pts1.size();
for ( int i=0; i q1 ( N ), q2 ( N ); // remove the center
for ( int i=0; i 
图 4: 轨迹对准前与对准后
Bibliography
- E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “Orb: an efficient alternative to sift or surf,” in
2011 IEEE International Conference on Computer Vision (ICCV), pp. 2564–2571, IEEE, 2011.
- P. L. Rosin, “Measuring corner properties,” Computer Vision and Image Understanding, vol. 73, no. 2, pp. 291–307, 1999.