走完SLAM十四讲前端之后，代码都已经注释完，但还是感觉有点迷茫，所以专门参考冯兵的博客，实现简单的视觉里程计。

收获是又重新认识到了C++基础的薄弱，决定之后的晚上要刷牛客题。不过就SLAM前端而言这部分基本可以理解代码了，这篇对VO代码进行注释。

基本过程：
1、获取图像
2、对图像进行处理
3、通过FAST算法对图像进行特征检测，通过KLT光流法跟踪图像的特征，如果跟踪的特征有所丢失，特征数小于某个阈值，那么重新特征检测。
4、通过RANSAC的5点算法来估计出两幅图像的本质矩阵。
5、通过本质矩阵进行估计R和t
6、对尺度信息进行估计，最终确定旋转矩阵和平移向量

先放效果图

转存失败重新上传取消

主程序main.cpp

#include <fstream>
#include <iostream>
#include <iomanip>
#include "visual_odometry.h"int main(int argc, char *argv[])
{//定义相机PinholeCamera *cam = new PinholeCamera(1241.0, 376.0,718.8560, 718.8560, 607.1928, 185.2157);//动态内存new返回相机类对象的指针//初始化对象voVisualOdometry vo(cam);// 用于保存轨迹数据std::ofstream out("position.txt");// 创建窗体用于显示读取的图片以及显示轨迹char text[100];int font_face = cv::FONT_HERSHEY_PLAIN;double font_scale = 1;int thickness = 1;cv::Point text_org(10, 50);cv::namedWindow("Road facing camera", cv::WINDOW_AUTOSIZE);cv::namedWindow("Trajectory", cv::WINDOW_AUTOSIZE);cv::Mat traj = cv::Mat::zeros(600, 600, CV_8UC3);// 用于绘制轨迹double x=0.0, y=0.0,z=0.0;// 用于保存轨迹 for (int img_id = 0; img_id < 2000; ++img_id){// 导入图像std::stringstream ss;ss <<  "F:/SLAM/dataset/KITTI/data_odometry_gray/00/image_1/"//单右斜线<< std::setw(6) << std::setfill('0') << img_id << ".png";//靠右对齐，字符长6，左边用0补充cv::Mat img(cv::imread(ss.str().c_str(), 0));assert(!img.empty());//如果没有照片就返回错误，终止程序// 处理帧vo.addImage(img, img_id);cv::Mat cur_t = vo.getCurrentT();if (cur_t.rows!=0){x = cur_t.at<double>(0);y = cur_t.at<double>(1);z = cur_t.at<double>(2);}out << x << " " << y << " " << z << std::endl;//中心点int draw_x = int(x) + 300;int draw_y = int(z) + 100;cv::circle(traj, cv::Point(draw_x, draw_y), 1, CV_RGB(255, 0, 0), 2);cv::rectangle(traj, cv::Point(10, 30), cv::Point(580, 60), CV_RGB(0, 0, 0), CV_FILLED);sprintf(text, "Coordinates: x = %02fm y = %02fm z = %02fm", x, y, z);cv::putText(traj, text, text_org, font_face, font_scale, cv::Scalar::all(255), thickness, 8);cv::imshow("Road facing camera", img);cv::imshow("Trajectory", traj);cv::waitKey(1);}delete cam;out.close();getchar();return 0;
}

区分 iostream与fstream与stringstream

iostream：是IO流的头文件，表明从流读入读出数据。这样才能用cin cout endl等等，

fstream：负责与文件输入输出打交道。包含着ifstream ofstream fstream三类。

下面先讲一下fstream。

ifstream表示从一个给定文件读取数据。ofstream表示向一个给定文件写入数据。

1、成员函数open()

首先根据三个类定义相应的对象，然后将对象与某个文件关联起来。

ifstream fin();
fin.open("text.txt");

void open ( const char * filename, ios_base::openmode mode = ios_base::in | ios_base::out );

它中间的参数是一个常量字符指针，也可以通过文件名的string常量

const string s="text.txt";
fin.open(s)

如果不是const string

string s="text.txt";
fin.open(s.str().c_str())//c_str()作用就是变成一个const string

2、在定义流对象的时候以文件名初始化

ifstream fin("test.txt");

例子：

#include <iostream>
#include <fstream>
#include <string>
using namespace std;int main(){ifstream in("test.txt"); // 建立in与文件test.txt之间的额关联if(!in)//判断是否加载进{cout << "error" << endl;return 1;}string line;while(getline(in, line))//按行读取txt中的内容，每次从fin指向的文件中读取一行，一行之中的所有字符都会被读入，包括空格。但是结尾的空格不读入，回车换行也不读入。{cout << line << endl;    }return 0;}

接下来讲一下stringstream

stringstream的对象与内存中的string对象建立关联，往string对象写东西，或者从string对象读取东西。

适用情况：处理行内单个单词

     // 导入图像std::stringstream ss;ss <<  "C:/dataset/00/image_1/"<< std::setw(6) << std::setfill('0') << img_id << ".png";cv::Mat img(cv::imread(ss.str().c_str(), 0));

str()成员函数的使用可以让istringstream对象返回一个string字符串

c_str()作用就是变成一个const string

动态内存new和delete

之前的程序中，所必需的内存空间的大小都是在程序执行之前就已经确定了。但是如果我们需要的内存是一个变量，数值只有在程序运行时才能确定，例如：需要根据用户的输入来决定内存空间。答案是动态内存分配(dynamic memory)，为此C++ 集成了操作符new 和delete。

1、简单的数值与数组内存

new的用法：new在动态内存堆中为对象分配空间并返回一个指向该对象的指针。

 new 数据类型 ;                  //申请内存空间。new 数据类型   (初值)；          //申请内存空间时，并指定该数据类型的初值。new 数据类型   [内存单元个数];    //申请多个内存空间。

int* a = new int (4);   //开辟一个int类型指针赋值给a，并地址中的内容赋值为4。
delete a;               //释放单个int的空间
int* b = new int[100];  //开辟一个大小为100的整型数组空间。
delete [] a;            //释放int数组空间

缺点：有时我们会忘记释放内存，这样就导致内存泄漏

2、智能指针动态管理内存

shared_ptr允许多个指针指向同一个对象，unique则是独占所指向的对象，weak_ptr是一种弱引用，指向shared_ptr管理的对象。

（1）智能指针是模板，当我们创建一个智能指针时候，必须提供额外信息------指针可以指向的类型，与vector一样，我们在<>中给出类型，之后就是所定义的这种智能指针的名字：

shared_ptr<string> p1;//shared_ptr可以指向string。

（2）shared_ptr与new结合

shared_ptr<int> p1(new int(1024))//直接初始化

相当于new返回的int* 来创建一个shared_ptr<int> 来。

导入照片：

std::stringstream ss;
ss <<  "F:/SLAM/dataset/image_1/"<< std::setw(6) << std::setfill('0') << img_id<< ".png";
cv::Mat img(cv::imread(ss.str().c_str(), 0));
assert(!img.empty());//如果没有照片就返回错误，终止程序

其中setw(6)表示图片名称是6个字符长，从右开始数，多余部分用0补充，例如：000006.png

ss.str().c_str()将string类型转为char*，为了与C兼容。

相机初始化：

//定义相机
PinholeCamera *cam = new PinholeCamera(1241.0, 376.0,718.8560, 718.8560, 607.1928, 185.2157);
//初始化对象vo
VisualOdometry vo(cam);

new创建一个PinholeCamera对象，并返回指向该对象的指针，cam是个指针。

在VisualOdometry类中对于相机定义是：

VisualOdometry::VisualOdometry(PinholeCamera* cam):cam_(cam)
{focal_ = cam_->fx();pp_ = cv::Point2d(cam_->cx(), cam_->cy());frame_stage_ = STAGE_FIRST_FRAME;
}

相机之后要使用的参数主要是：焦距、相机中心位移、当前帧状态。

关键的处理帧这里只用到了addImage与getCurrentT()，下一节讨论
画面呈现

if (cur_t.rows!=0)
{x = cur_t.at<double>(0);y = cur_t.at<double>(1);z = cur_t.at<double>(2);
}
int draw_x = int(x) + 300;
int draw_y = int(z) + 100;
cv::circle(traj, cv::Point(draw_x, draw_y), 1, CV_RGB(255, 0, 0), 2);
cv::rectangle(traj, cv::Point(10, 30), cv::Point(580, 60), CV_RGB(0, 0, 0), CV_FILLED);
sprintf(text, "Coordinates: x = %02fm y = %02fm z = %02fm", x, y, z);
cv::putText(traj, text, text_org, font_face, font_scale, cv::Scalar::all(255), thickness, 8);
cv::imshow("Road facing camera", img);
cv::imshow("Trajectory", traj);

要想把轨迹画出来，先将x转为int类型，之后(300,100)是起始点的坐标。

cv::Mat traj = cv::Mat::zeros(600, 600, CV_8UC3);// 用于绘制轨迹，在视频当中看到黑色的画布。

sprintf输出x,y,z的文本以及在画面上呈现文字putText().

visual_odometry.h

#ifndef VISUAL_ODOMETRY_H_
#define VISUAL_ODOMETRY_H_#include <vector>
#include <opencv2/opencv.hpp>
#include "pinhole_camera.h"class VisualOdometry
{
public://目前所在帧，通过限制条件更好的确定第一帧和第二帧图像用于确定初始位置enum FrameStage {STAGE_FIRST_FRAME,//第一帧STAGE_SECOND_FRAME,//第二帧STAGE_DEFAULT_FRAME//默认帧};//构造函数，析构函数VisualOdometry(PinholeCamera* cam);virtual ~VisualOdometry();/// 提供一个图像void addImage(const cv::Mat& img, int frame_id);/// 获取当前帧的旋转cv::Mat getCurrentR() { return cur_R_; }/// 获得当前帧的平移cv::Mat getCurrentT() { return cur_t_; }protected: /// 处理第一帧virtual bool processFirstFrame();/// 处理第二帧virtual bool processSecondFrame();/// 处理完开始的两个帧后处理所有帧virtual bool processFrame(int frame_id);/// 计算绝对尺度double getAbsoluteScale(int frame_id);/// 特征检测void featureDetection(cv::Mat image, std::vector<cv::Point2f> &px_vec);/// 特征跟踪void featureTracking(cv::Mat image_ref, cv::Mat image_cur,std::vector<cv::Point2f>& px_ref, std::vector<cv::Point2f>& px_cur, std::vector<double>& disparities);protected:FrameStage frame_stage_;                 //!< 当前为第几帧PinholeCamera *cam_;                     //!< 相机cv::Mat new_frame_;                      //!< 当前帧cv::Mat last_frame_;                     //!< 最近处理帧cv::Mat cur_R_;//!< 计算出当前的姿态，后期将图像和姿态进行封装为帧cv::Mat cur_t_;//!< 当前的平移std::vector<cv::Point2f> px_ref_;      //!< 在参考帧中用于跟踪的特征点std::vector<cv::Point2f> px_cur_;      //!< 在当前帧中跟踪的特征点std::vector<double> disparities_;      //!< 第一帧与第二帧对应光流跟踪的特征之间的像素差值double focal_;//!<相机焦距cv::Point2d pp_; //!<相机中心点};#endif // VISUAL_ODOMETRY_H_

先从addImage开始

void VisualOdometry::addImage(const cv::Mat& img, int frame_id)
{//对添加的图像进行判断if (img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height())throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale");// 添加新帧 new_frame_ = img;bool res = true;//结果状态if (frame_stage_ == STAGE_DEFAULT_FRAME)res = processFrame(frame_id);else if (frame_stage_ == STAGE_SECOND_FRAME)res = processSecondFrame();else if (frame_stage_ == STAGE_FIRST_FRAME)res = processFirstFrame();// 将当前帧设为上一处理帧last_frame_ = new_frame_;}

根据当前帧的状态分别放进三个函数里，第一帧、第二帧、正常帧

正常帧processFrame

//之后的帧R、t要累加
bool VisualOdometry::processFrame(int frame_id)
{double scale = 1.00;//初始尺度为1featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_); //通过光流跟踪确定第二帧中的相关特征cv::Mat E, R, t, mask;E = cv::findEssentialMat(px_cur_, px_ref_, focal_, pp_, cv::RANSAC, 0.999, 1.0, mask);cv::recoverPose(E, px_cur_, px_ref_, R, t, focal_, pp_, mask);scale = getAbsoluteScale(frame_id);//得到当前帧的实际尺度if (scale > 0.1) //如果尺度小于0.1可能计算出的Rt存在一定的问题,则不做处理，保留上一帧的值{cur_t_ = cur_t_ + scale*(cur_R_*t);cur_R_ = R*cur_R_;}// 如果跟踪特征点数小于给定阈值，进行重新特征检测if (px_ref_.size() < kMinNumFeature){featureDetection(new_frame_, px_ref_);featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_);}//当前特征点变为参考帧特征点px_ref_ = px_cur_;return true;
}

第一帧processFirstFrame()

// 第一帧只有特征检测，计算出来的特征为参考帧特征点
bool VisualOdometry::processFirstFrame()
{featureDetection(new_frame_, px_ref_);// 修改状态，表明第一帧已经处理完成frame_stage_ = STAGE_SECOND_FRAME;return true;
}

第二帧processSecondFrame()

// 第二帧算出第一次R，t，特征跟踪、基础矩阵、恢复位姿
bool VisualOdometry::processSecondFrame()
{featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_); //通过光流跟踪确定第二帧中的相关特征cv::Mat E, R, t, mask;E = cv::findEssentialMat(px_cur_, px_ref_, focal_, pp_, cv::RANSAC, 0.999, 1.0, mask);cv::recoverPose(E, px_cur_, px_ref_, R, t, focal_, pp_, mask);cur_R_ = R.clone();cur_t_ = t.clone();// 设置状态处理默认帧frame_stage_ = STAGE_DEFAULT_FRAME;//当前特征点变为参考帧特征点px_ref_ = px_cur_;return true;
}

作者开源的代码可以跑通，注释为：


#include "vo_features.h"using namespace cv;
using namespace std;
//define宏定义，全局变量
#define MAX_FRAME 1000
#define MIN_NUM_FEAT 2000
//获得绝对尺度？？？
double getAbsoluteScale(int frame_id, int sequence_id, double z_cal)    {string line;int i = 0;ifstream myfile ("/home/avisingh/Datasets/KITTI_VO/00.txt");double x =0, y=0, z = 0;double x_prev, y_prev, z_prev;if (myfile.is_open()){while (( getline (myfile,line) ) && (i<=frame_id)){z_prev = z;x_prev = x;y_prev = y;std::istringstream in(line);//cout << line << '\n';for (int j=0; j<12; j++)  {in >> z ;if (j==7) y=z;if (j==3)  x=z;}i++;}myfile.close();}else {cout << "Unable to open file";return 0;}return sqrt((x-x_prev)*(x-x_prev) + (y-y_prev)*(y-y_prev) + (z-z_prev)*(z-z_prev)) ;}int main( int argc, char** argv )  Mat img_1, img_2;Mat R_f, t_f;//生成输出的文件ofstream myfile;myfile.open ("results1_1.txt");//后面会采用帧数数据计算两帧之间距离作为scaledouble scale = 1.00;//先创建数组，sprintf()将照片输出到指定指针,前两张图像char filename1[200];char filename2[200];sprintf(filename1, "/home/avisingh/Datasets/KITTI_VO/00/image_2/%06d.png", 0);sprintf(filename2, "/home/avisingh/Datasets/KITTI_VO/00/image_2/%06d.png", 1);//读数据集前两帧Mat img_1_c = imread(filename1);Mat img_2_c = imread(filename2);if ( !img_1_c.data || !img_2_c.data ) { std::cout<< " --(!) Error reading images " << std::endl; return -1;}// 转为灰度图像cvtColor(img_1_c, img_1, COLOR_BGR2GRAY);cvtColor(img_2_c, img_2, COLOR_BGR2GRAY);// 特征检测，追踪vector<Point2f> points1, points2;        //vectors to store the coordinates of the feature pointsfeatureDetection(img_1, points1);        //detect features in img_1vector<uchar> status;featureTracking(img_1,img_2,points1,points2, status); //track those features to img_2//相机参数double focal = 718.8560;cv::Point2d pp(607.1928, 185.2157);//找E，恢复第二帧位姿T[R|t]Mat E, R, t, mask;E = findEssentialMat(points2, points1, focal, pp, RANSAC, 0.999, 1.0, mask);recoverPose(E, points2, points1, R, t, focal, pp, mask);R_f = R.clone();t_f = t.clone();// 创建窗体用于显示读取的图片以及显示轨迹char text[100];int fontFace = FONT_HERSHEY_PLAIN;double fontScale = 1;int thickness = 1;cv::Point textOrg(10, 50);namedWindow( "Road facing camera", WINDOW_AUTOSIZE );// 创建窗口namedWindow( "Trajectory", WINDOW_AUTOSIZE );Mat traj = Mat::zeros(600, 600, CV_8UC3);//创建黑背景//以第二帧作为循环的第一帧图像Mat prevImage = img_2;Mat currImage;vector<Point2f> prevFeatures = points2;vector<Point2f> currFeatures;char filename[100];for(int numFrame=2; numFrame < MAX_FRAME; numFrame++) {sprintf(filename, "/home/avisingh/Datasets/KITTI_VO/00/image_2/%06d.png", numFrame);//cout << numFrame << endl;Mat currImage_c = imread(filename);cvtColor(currImage_c, currImage, COLOR_BGR2GRAY);vector<uchar> status;featureTracking(prevImage, currImage, prevFeatures, currFeatures, status);E = findEssentialMat(currFeatures, prevFeatures, focal, pp, RANSAC, 0.999, 1.0, mask);recoverPose(E, currFeatures, prevFeatures, R, t, focal, pp, mask);//特征点化为二维像素坐标(xy)，有什么用吗？？？可以直接删掉Mat prevPts(2,prevFeatures.size(), CV_64F), currPts(2,currFeatures.size(), CV_64F);for(int i=0;i<prevFeatures.size();i++)    {   prevPts.at<double>(0,i) = prevFeatures.at(i).x;prevPts.at<double>(1,i) = prevFeatures.at(i).y;currPts.at<double>(0,i) = currFeatures.at(i).x;currPts.at<double>(1,i) = currFeatures.at(i).y;}//获得绝对尺度，最后一个参数是zscale = getAbsoluteScale(numFrame, 0, t.at<double>(2));//z坐标大于x y且尺度大于0.1？？？if ((scale>0.1)&&(t.at<double>(2) > t.at<double>(0)) && (t.at<double>(2) > t.at<double>(1))) {t_f = t_f + scale*(R_f*t);R_f = R*R_f; }else {cout << "scale below 0.1, or incorrect translation" << endl;}//输出轨迹数据，形式如00.txtmyfile << t_f.at<double>(0) << " " << t_f.at<double>(1) << " " << t_f.at<double>(2) << endl;// 如果被跟踪的特征数量低于特定阈值，则触发重新检测if (prevFeatures.size() < MIN_NUM_FEAT) {cout << "Number of tracked features reduced to " << prevFeatures.size() << endl;cout << "trigerring redection" << endl;featureDetection(prevImage, prevFeatures);featureTracking(prevImage,currImage,prevFeatures,currFeatures, status);}prevImage = currImage.clone();prevFeatures = currFeatures;//绘制轨迹，为什么y的是第三个z，汽车相机朝向正前方//Mat.at<>()访问元素int x = int(t_f.at<double>(0)) + 300;int y = int(t_f.at<double>(2)) + 100;//画圆(图像，圆心坐标，半径,红色，线条粗细)circle(traj, Point(x, y) ,1, CV_RGB(255,0,0), 2);//矩形（图像，对角的顶点，黑色，填充），放文字前先用矩形覆盖住。rectangle( traj, Point(10, 30), Point(550, 50), CV_RGB(0,0,0), CV_FILLED);//坐标实时变化sprintf(text, "Coordinates: x = %02fm y = %02fm z = %02fm", t_f.at<double>(0), t_f.at<double>(1), t_f.at<double>(2));//放文字（图像你，文字内容，位置，字体类型，颜色白，厚度）putText(traj, text, textOrg, fontFace, fontScale, Scalar::all(255), thickness, 8);imshow( "Road facing camera", currImage_c );imshow( "Trajectory", traj );waitKey(1);}//计时结束，转化为以秒为单位clock_t end = clock();double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;cout << "Total time taken: " << elapsed_secs << "s" << endl;//cout << R_f << endl;//cout << t_f << endl;return 0;
}

先要匹配上对应特征点，之后恢复[R|t] 关键代码为：

E = findEssentialMat(currFeatures, prevFeatures, focal, pp, RANSAC, 0.999, 1.0, mask);
recoverPose(E, currFeatures, prevFeatures, R, t, focal, pp, mask);

为了下面的式子成立，需要提前通过前两帧求得[R|t],得到 R_f = R.clone(); t_f = t.clone();

t_f = t_f + scale*(R_f*t);

R_f = R*R_f;

特征检测用的Fast角点，跟踪使用KLT光流法，但是这里的代码显得很不友好，还是用之前SLAM十四讲里的清晰些。

接下来要抓紧把后端以及见图部分搞定，deadline是这个月末，已经拖了很久了，像师兄讲的你得先快速的过一遍然后再找感兴趣的点进行深究。现在只是前端内容还远远不够。

VO：简单的视觉里程计代码注释（代码可运行）相关推荐

一些基于深度学习的视觉里程计/SLAM开源代码
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:黄浴 https://zhuanlan.zhihu.com/p/139150194 本文转载自知 ...
最新汇总：一些基于深度学习的视觉里程计/SLAM开源代码
点上方蓝字计算机视觉联盟获取更多干货在右上方 ··· 设为星标 ★,与你不见不散仅作分享,不代表本公众号立场,侵权联系删除转载于:黄浴博士知乎,已获授权,https://zhuanlan.zhi ...
什么是视觉里程计（Visual Odometry）？
1.概念:什么是里程计? 在里程计问题中,我们希望测量一个运动物体的轨迹.这可以通过许多不同的手段来实现.例如,我们在汽车轮胎上安装计数码盘,就可以得到轮胎转动的距离,从而得到汽车的估计.或者,也可以 ...
视觉SLAM：ORB视觉里程计的简单实现(一)
ORB视觉里程计的简单实现之一概述一.基础视觉里程计的算法实现步骤二.具体实现细节 1.公用 2.相机类 3.图像帧 4.系统类 5.视觉里程计类 6.其他 6.1 显示类 6.2 地图点类 6 ...
去掉左边0_SLAM从0到1——11. 视觉里程计VO内容框架
「本文是之前学习VO 部分整理的思维导图,笔记写入之后均折叠了起来,正文中采用markdown格式展开,可看到笔记内容」放上来的目的其实是方便自己查阅笔记,同样给有需要的同学提供一点思路. 整体上的 ...
零基础从零开始写VO视觉里程计
零基础从零开始写VO代码课程以直播写代码讲理论为主, 提升代码编程能力和VO视觉里程计理论知识. 数学基础:微分积分.线性代数.矩阵论.统计学.概率论.最优化等操作系统:Linux(Ubuntu) ...
【slam十四讲第二版】【课本例题代码向】【第七讲~视觉里程计Ⅱ】【使用LK光流（cv）】【高斯牛顿法实现单层光流和多层光流】【实现单层直接法和多层直接法】
[slam十四讲第二版][课本例题代码向][第七讲~视觉里程计Ⅱ][使用LK光流(cv)][高斯牛顿法实现单层光流和多层光流][实现单层直接法和多层直接法] 0 前言 1 使用LK光流(cv) 1.1 ...
视觉里程计Visual Odometry(VO)
视觉里程计(Visual Odometry,简称VO)是SLAM技术中非常关键的部分,主要侧重于计算机视觉算法. 视觉里程计这个术语借鉴了汽车的车轮里程计的概念,还是挺贴切的. 我们知道汽车的车轮里程 ...
视觉里程计-代码仓库
1. 基于opencv的视觉里程计(visual odometry) GitHub - Transportation-Inspection/visual_odometry: visual odomet ...

VO：简单的视觉里程计代码注释（代码可运行）

主程序main.cpp

区分 iostream与fstream与stringstream

动态内存new和delete

visual_odometry.h

VO：简单的视觉里程计代码注释（代码可运行）相关推荐

最新文章

热门文章