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

走完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返回相机类对象的指针
	//初始化对象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, i