#优质创作者#实现一个简单的视觉里程计 原创

【本文正在参加优质创作者激励计划】

视觉里程计基本过程：
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返回相机类对象的指针
	//初始化对象vo
	VisualOdometry 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;//初始尺度为1
	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);
	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");
  //后面会采用帧数数据计算两帧之间距离作为scale
  double 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 points
  featureDetection(img_1, points1);        //detect features in img_1
  vector<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;
    }
	//获得绝对尺度，最后一个参数是z
  	scale = 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.txt
   myfile << 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十四讲里的清晰些。