视觉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<cv::KeyPoint> &keypoints) {int half_patch_size = 8;for (auto &kp : keypoints) {// START YOUR CODE HERE (~7 lines)//judge if keypoint is on edgeint 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=0for(int i=-half_patch_size;i<half_patch_size;i++){ //-8<i<8,-8<j<8for(int j=-half_patch_size;j<half_patch_size;j++){m01 += j*image.at<uchar>(y+j,x+i); //真实坐标(j,i)+(y,x)m10 += i*image.at<uchar>(y+j,x+i); //获得单个像素值image.at<uchar>(y,x)}}kp.angle = atan(m01/m10)*180/pi;cout<<"m10 = "<<m01<<" "<<"m01 = "<<m10<<" "<<"kp.angle = "<<kp.angle<<endl;// END YOUR CODE HERE}return;
}
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<cv::KeyPoint> &keypoints,vector<DescType> &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<uchar>(p)>image.at<uchar>(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<DescType> &desc1, const vector<DescType> &desc2,vector<cv::DMatch> &matches) {int d_max = 50;// START YOUR CODE HERE (~12 lines)// find matches between desc1 and desc2.for(int i=0;i<desc1.size();i++){if(desc1[i].empty())continue;int d_min=256 ,index=-1; //必须定义在这里,每次循环重新初始化for(int j=0;j<desc2.size();j++){ //这个for循环,取出最小的d_minif(desc2[j].empty())continue;int d=0; //必须定义在这里,每次循环重新初始化for(int k=0;k<256;k++){d += desc1[i][k]^desc2[j][k]; //异或:不同为1;}if(d<d_min){d_min=d;index=j;}}if(d_min<=d_max){cv::DMatch match(i,index,d_min);matches.push_back(match);}}// END YOUR CODE HEREfor (auto &m : matches) {cout << m.queryIdx << ", " << m.trainIdx << ", " << m.distance << endl;}return;
}
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<cv::KeyPoint> &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 HEREint 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=0for(int i=-half_patch_size;i<half_patch_size;i++){ //-8<i<8,-8<j<8for(int j=-half_patch_size;j<half_patch_size;j++){m01 += j*image.at<uchar>(y+j,x+i); //真实坐标(j,i)+(y,x)m10 += i*image.at<uchar>(y+j,x+i); //获得单个像素值image.at<uchar>(y,x)}}kp.angle = atan(m01/m10)*180/pi;cout<<"m10 = "<<m01<<" "<<"m01 = "<<m10<<" "<<"kp.angle = "<<kp.angle<<endl;}// END YOUR CODE HERE});return;
}
void computeORBDescMT(const cv::Mat &image, vector<cv::KeyPoint> &keypoints,vector<DescType> &desc) {// START YOUR CODE HERE (~20 lines)
std::for_each(std::execution::par_unseq,keypoints.begin(),keypoints.end(),
[&image,&desc](auto &kp){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<uchar>(p)>image.at<uchar>(q)?0:1; // END YOUR CODE HERE}std::lock_guard<std::mutex>guard(m);desc.push_back(d);});// END YOUR CODE HERE
}
二.从 E 恢 复 R, t (3 分,约 1 小时)
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>using namespace Eigen;#include <sophus/so3.hpp>#include <iostream>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,tMatrix3d R;Vector3d t;// SVD and fix sigular values// START YOUR CODE HEREJacobiSVD<MatrixXd> 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"<<SIGMA<<endl;cout<<"sigma_value=\n"<<sigma_value<<endl;cout<<"SIGMA2=\n"<<SIGMA2<<endl;cout<<"sigma_value2=\n"<<sigma_value2<<endl;// END YOUR CODE HERE// set t1, t2, R1, R2 // START YOUR CODE HEREMatrix3d t_wedge1;Matrix3d t_wedge2;Matrix3d R1;Matrix3d R2;Matrix3d RZ1=AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();Matrix3d RZ2=AngleAxisd(-M_PI/2,Vector3d(0,0,1)).toRotationMatrix();t_wedge1=U*RZ1*SIGMA2*U.transpose();t_wedge2=U*RZ2*SIGMA2*U.transpose();R1=U*RZ1.transpose()*V.transpose();R2=U*RZ2.transpose()*V.transpose();// END YOUR CODE HEREcout << "R1 = " << R1 << endl;cout << "R2 = " << R2 << endl;cout << "t1 = " << Sophus::SO3d::vee(t_wedge1) << endl;cout << "t2 = " << Sophus::SO3d::vee(t_wedge2) << endl;// check t^R=E up to scaleMatrix3d tR = t_wedge1 * R1;cout << "t^R = " << tR << endl;return 0;
}
三.用 G-N 实现 Bundle Adjustment 中的位姿估计 (3 分 , 约 2 小时)
答:
1.
2.
3.左乘exp(dx),扰动模型
#include <Eigen/Core>
#include <Eigen/Dense>using namespace Eigen;#include <vector>
#include <fstream>
#include <iostream>
#include <iomanip>#include "sophus/se3.hpp"using namespace std;typedef vector<Vector3d, Eigen::aligned_allocator<Vector3d>> VecVector3d;
typedef vector<Vector2d, Eigen::aligned_allocator<Vector3d>> VecVector2d;
typedef Matrix<double, 6, 1> 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 HEREifstream fin(p3d_file);if(!fin){cout<<"不能打开文件"<<endl;return 1;}while(!fin.eof()){double x,y,z;fin>>x>>y>>z;Vector3d v(x,y,z);p3d.push_back(v);}ifstream fins(p2d_file);if(!fins){cout<<"不能打开文件"<<endl;return 1;}while(!fins.eof()){double x,y;fins>>x>>y;Vector2d v(x,y);p2d.push_back(v);}// END YOUR CODE HEREassert(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 posefor (int iter = 0; iter < iterations; iter++) {Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();Vector6d b = Vector6d::Zero();cost = 0;// compute costfor (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 jacobianMatrix<double, 2, 6> 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 HEREH += J.transpose() * J;b += -J.transpose() * e;}// solve dx Vector6d dx;// START YOUR CODE HERE dx=H.ldlt().solve(b);// END YOUR CODE HEREif (isnan(dx[0])) {cout << "result is nan!" << endl;break;}if (iter > 0 && cost >= lastCost) {// cost increase, update is not goodcout << "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 HERElastCost = 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 <sophus/se3.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <cmath>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>using namespace std;
using namespace Eigen;
using namespace cv;
void ReadData(string FileName ,vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses_e,vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses_g,vector<Point3f> &t1,vector<Point3f> &t2
);
void icp_svd (const vector<Point3f>& pts1,const vector<Point3f>& pts2,Matrix3d & R,Vector3d& t);
void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_e,vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g);int main(int argc,char **argv){string TrajectoryFile = "./compare.txt";vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_e;vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g;vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g_; //poses_g_=T*poses_gEigen::Matrix3d R;Eigen::Vector3d t;vector<Point3f> 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<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses_e,vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses_g,vector<Point3f> &t_e,vector<Point3f> &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!"<<endl;return ;}while(getline(fin,line)){istringstream record(line);record>>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<Point3f>& pts1,const vector<Point3f>& pts2,Matrix3d& R, Vector3d& t) {Point3f p1, p2; // center of massint 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 centerfor ( int i=0; i<N; i++ ){q1[i] = pts1[i] - p1;q2[i] = pts2[i] - p2;}// compute q1*q2^TEigen::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 WEigen::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;R = U* ( V.transpose() ); //p1=R_12*p_2,注意R的意义,p2到p1的旋转关系t = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R * Eigen::Vector3d ( p2.x, p2.y, p2.z );
}/*****************************绘制轨迹*******************************************/
void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_e,vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g) {if (poses_g.empty() || poses_e.empty()) {cerr << "Trajectory is empty!" << endl;return;}// create pangolin window and plot the trajectorypangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768); //创建一个窗口glEnable(GL_DEPTH_TEST); //启动深度测试glEnable(GL_BLEND); //启动混合glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); //混合函数glBlendFunc( GLenum sfactor , GLenum dfactor );sfactor 源混合因子dfactor 目标混合因子pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0) //对应的是gluLookAt,摄像机位置,参考点位置,up vector(上向量));pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f).SetHandler(new pangolin::Handler3D(s_cam));while (pangolin::ShouldQuit() == false) {glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);glClearColor(1.0f, 1.0f, 1.0f, 1.0f);glLineWidth(2);for (size_t i = 0; i < poses_e.size() - 1; i++) {glColor3f(1 - (float) i / poses_e.size(), 0.0f, (float) i / poses_e.size());glBegin(GL_LINES);auto p1 = poses_e[i], p2 = poses_e[i + 1];glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);glEnd();}for (size_t j = 0; j < poses_g.size() - 1; j++) {glColor3f(1 - (float) j / poses_g.size(), 0.0f, (float) j / poses_g.size());glBegin(GL_LINES);auto p1 = poses_g[j], p2 = poses_g[j + 1];glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);glEnd();}pangolin::FinishFrame();}}
图 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.
视觉slam学习笔记以及课后习题《第五讲特征点法视觉里程计》相关推荐
- 视觉slam学习笔记以及课后习题《第三讲李群李代数》
前言 这篇博客主要记录了我在深蓝学院视觉slam课程中的课后习题,因为是为了统计知识点来方便自己以后查阅,所以有部分知识可能不太严谨,如果给大家造成了困扰请见谅,大家发现了问题也可以私信或者评论给我及 ...
- 视觉SLAM学习笔记
中英文对照表 中文 英文 计算机视觉 Computer Vision 人工智能 Artificial Intelligence 单目相机 Monocular 双目相机 Stereo 深度相机 RGB- ...
- 《C语言程序设计》谭浩强-学习笔记以及课后习题答案(考前复习/考研/专升本)
此笔记是几年前为了本人考试而学而写,今日回首感慨良多,便把尘封多年在旧电脑中的学习笔记翻出来分享给大家 此笔记参考书籍: <C语言程序设计>谭浩强 根据前九章内容主要知识点进行梳理 如果有 ...
- Ubuntu LInux操作系统 学习笔记及课后习题解答
1.ubuntu基本使用 1.1 GNU GPL GNU通用公共许可证(general public license),开放.自由的精神,任何软件加上GPL协议后,即成为自由的软件,任何人均可获得,同 ...
- 《视觉SLAM十四讲 第二版》笔记及课后习题(第一讲)
前言 之所以想要写这个系列的博客,是因为想要总结一下高博的<SLAM视觉十四讲第二版>的各章内容以及自己对书后习题的一些做法,也算是对自己学习过程的一个总结和回顾.博客分为两个大部分,即读 ...
- 数字电子技术基础第三版杨志忠_阎石数字电子技术基础第6版笔记和课后习题详解...
阎石<数字电子技术基础>(第6版)笔记和课后习题(含考研真题)详解 第1章 数制和码制 1.1 复习笔记 本章作为<数字电子技术基础>的开篇章节,是数字电路学习的基础.本章介绍 ...
- 范里安中级微观经济学(第9版)分析笔记和课后习题答案解析-完整版 范里安《微观经济学:现代观点》(第9版)笔记和课后习题详解!
范里安中级微观经济学(第9版)分析笔记和课后习题答案解析-完整版 摘自硕达学习网 范里安<微观经济学:现代观点>(第9版)笔记和课后习题详解! 最新电子书(题库) 范里安微观经济学现代观 ...
- [杨可桢]机械设计基础题库 机械设计基础习题 2022机械设计基础考试题答案 杨可桢《机械设计基础》(第7版)笔记和课后习题(含考研真题)详解
[杨可桢]机械设计基础题库 机械设计基础习题 2022机械设计基础考试题答案-硕达学习网 杨可桢<机械设计基础>(第7版)笔记和课后习题(含考研真题)详解 目录 第1章 平面机构的自由 ...
- SLAM学习笔记(十九)开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
本文为我在浙江省北大信研院-智能计算中心-情感智能机器人实验室-科技委员会所做的一个分享汇报,现在我把它搬运到博客中. 由于参与分享汇报的同事有许多是做其他方向的机器人工程师(包括硬件.控制等各方面并 ...
最新文章
- 二、Windows下TortoiseGit的安装与配置
- java中的foreach语句
- HttpRunner自动化框架学习笔记
- php正文重复度,百度如何判断网页文章的重复度?两个页面相似度确认方法介绍...
- 利用子网掩码划分子网
- 9 年前他用 1 万个比特币买两个披萨, 9 年后他把当年的代码卖给苹果, 成 GPU 挖矿之父...
- java 获得平台编码_关于Java平台的编码
- 最强悍的FCKEditor配置和攻略(转载)
- 洛谷 P1400 塔
- EXTASP.Net几天使用总结
- scrapy 中不同页面的拼接_Python下使用Scrapy爬取网页内容的实例
- 微信读书爬虫 wereader
- C#模拟鼠标和键盘操作
- 转载:Python中to_csv函数输出的utf8数据用Excel打开是乱码
- PMI-ACP练习题(22)
- 企业绩效管理怎么做?
- Android检测仪开发---BleBluetooth 多连接
- kubesphere安装Maven+JDK17 流水线打包
- COM高级应用-Automation(自动化)已是昨日黄花不再高级?
- python调用cplex求解装箱问题_装箱问题的CPLEX求解
热门文章
- Codeforces Round #493 (Div. 2):C. Convert to Ones
- pytorch 中 torch.optim.Adam 方法的使用和参数的解释
- kite:Python 代码自动补全神器
- zeppelin k8s安装部署和基本使用教程(在线的分析工具)
- postman使用教程,接口自动化测试
- java实现使用JDBC-ODBC桥操作数据库。
- jQuery Mobile中尾部栏footer的data-*选项
- svg的transform-matrix详解
- 购物车项目 复杂版本.待简化
- Scanner类中的next()和nextLine()方法