
#优质创作者#实现一个简单的视觉里程计 原创
【本文正在参加优质创作者激励计划】
视觉里程计基本过程:
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十四讲里的清晰些。
