文章目录
一、代码解释:
0.3版的vo在0.2的基础上加上了g2o优化
在visual_odometry.cpp中通过调用cv::solvePnPRansac()方法求出R,t过后再用g2o,位姿进行优化。
在g2o_types.h定义了三种边但是只有一种符合3D到2D的重投影误差。
g2o_types.h部分代码
#ifndef MYSLAM_G2O_TYPES_H
#define MYSLAM_G2O_TYPES_H
#include "myslam/common_include.h"
#include "camera.h"
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
namespace myslam {
...
//第三种
class EdgeProjectXYZ2UVPoseOnly
: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
public:
//Eigen自动内存对齐
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//计算误差函数
virtual void computeError();
//线性优化方法(计算6 * 2 雅克比矩阵)
virtual void linearizeOplus();
//读写功能函数
virtual bool read(std::istream &in) {}
virtual bool write(std::ostream &os) const {};
//定义三维点坐标和相机模型(计算雅克比矩阵需要用到x,y,z)
Vector3d point_;
Camera *camera_;
};
}
#endif // MYSLAM_G2O_TYPES_H
g2o_types.cpp部分代码
#include "myslam/g2o_types.h"
namespace myslam {
...
//计算误差
void EdgeProjectXYZ2UVPoseOnly::computeError() {
//取出顶点转换成位姿
const g2o::VertexSE3Expmap *pose = static_cast<const g2o::VertexSE3Expmap *> ( _vertices[0] );
//误差计算 : 测量值 - 估计值 (重投影误差)
//原理:由位姿转换为四元数在由四元素映射成相机坐标,在利用camera类中的方法映射到像素坐标
_error = _measurement - camera_->camera2pixel(
pose->estimate().map(point_));
}
//线性优化方法
void EdgeProjectXYZ2UVPoseOnly::linearizeOplus() {
//顶点数组中取出顶点,转换成位姿(顶点虽然里面存的是位姿但是还是涉及到类型转换)
g2o::VertexSE3Expmap *pose = static_cast<g2o::VertexSE3Expmap *> ( _vertices[0] );
//这由位姿构造一个四元数形式T
g2o::SE3Quat T(pose->estimate());
//用 T求得变换后的3D点坐标
Vector3d xyz_trans = T.map(point_);
//拿到了3D点的x,y,z方便接下来计算雅克比矩阵
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z * z;
//这里的雅克比矩阵和14讲里面有点不一样
_jacobianOplusXi(0, 0) = x * y / z_2 * camera_->fx_;
_jacobianOplusXi(0, 1) = -(1 + (x * x / z_2)) * camera_->fx_;
_jacobianOplusXi(0, 2) = y / z * camera_->fx_;
_jacobianOplusXi(0, 3) = -1. / z * camera_->fx_;
_jacobianOplusXi(0, 4) = 0;
_jacobianOplusXi(0, 5) = x / z_2 * camera_->fx_;
_jacobianOplusXi(1, 0) = (1 + y * y / z_2) * camera_->fy_;
_jacobianOplusXi(1, 1) = -x * y / z_2 * camera_->fy_;
_jacobianOplusXi(1, 2) = -x / z * camera_->fy_;
_jacobianOplusXi(1, 3) = 0;
_jacobianOplusXi(1, 4) = -1. / z * camera_->fy_;
_jacobianOplusXi(1, 5) = y / z_2 * camera_->fy_;
}
}
接下来看最重要的visual_odometry.h及其实现
#ifndef VISUALODOMETRY_H
#define VISUALODOMETRY_H
#include "myslam/common_include.h"
#include "myslam/map.h"
#include <opencv2/features2d/features2d.hpp>
namespace myslam {
class VisualOdometry {
public:
typedef shared_ptr <VisualOdometry> Ptr;
//枚举表征VO状态,初始化、正常、丢失
enum VOState {
INITIALIZING = -1,
OK = 0,
LOST
};
//VO状态、地图(关键帧和特征点)、参考帧、当前帧
VOState state_; // current VO status
Map::Ptr map_; // map with all frames and map points
Frame::Ptr ref_; // reference key-frame
Frame::Ptr curr_; // current frame
//orb、前一帧 3D点、当前帧关键点、前一帧和当前帧的描述子矩阵、前一帧后一帧的关键点匹配关系
cv::Ptr <cv::ORB> orb_; // orb detector and computer
vector <cv::Point3f> pts_3d_ref_; // 3d points in reference frame
vector <cv::KeyPoint> keypoints_curr_; // keypoints in current frame
Mat descriptors_curr_; // descriptor in current frame
Mat descriptors_ref_; // descriptor in reference frame
vector <cv::DMatch> feature_matches_; // feature matches
//特征匹配提取器
cv::FlannBasedMatcher matcher_flann_; // flann matcher
//位姿估计T矩阵、内点数、丢失数
SE3 T_c_r_estimated_; // the estimated pose of current frame
int num_inliers_; // number of inlier features in icp
int num_lost_; // number of lost times
// parameters
//特征点数量、尺度、金字塔等级数、特征点好的匹配数量、最大持续时间丢失、最小内点数量、
int num_of_features_; // number of features
double scale_factor_; // scale in image pyramid
int level_pyramid_; // number of pyramid levels
float match_ratio_; // ratio for selecting good matches
int max_num_lost_; // max number of continuous lost times
int min_inliers_; // minimum inliers
double key_frame_min_rot; // minimal rotation of two key-frames
double key_frame_min_trans; // minimal translation of two key-frames
double map_point_erase_ratio_; // remove map point ratio
//构造函数析构函数
public: // functions
VisualOdometry();
~VisualOdometry();
//核心!!!添加新关键帧
bool addFrame(Frame::Ptr frame); // add a new frame
protected:
// inner operation.
void extractKeyPoints(); //提取特征点
void computeDescriptors();//计算描述子
void featureMatching();//计算特征匹配
void setRef3DPoints();//还原深度信息
void poseEstimationPnP();//PNP
//关键帧相关的函数
void addKeyFrame();
bool checkEstimatedPose();
bool checkKeyFrame();
};
}
#endif // VISUALODOMETRY_H
visual_odometry.cpp
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <boost/timer.hpp>
#include "myslam/config.h"
#include "myslam/visual_odometry.h"
#include "myslam/g2o_types.h"
namespace myslam {
//初始化列表,生成对象的时候自动调用
VisualOdometry::VisualOdometry() :
state_(INITIALIZING), ref_(nullptr), curr_(nullptr), map_(new Map), num_lost_(0), num_inliers_(0),
matcher_flann_(new cv::flann::LshIndexParams(5, 10, 2)) {
//从配置文件中加载VO配置
num_of_features_ = Config::get<int>("number_of_features");
scale_factor_ = Config::get<double>("scale_factor");
level_pyramid_ = Config::get<int>("level_pyramid");
match_ratio_ = Config::get<float>("match_ratio");
max_num_lost_ = Config::get<float>("max_num_lost");
min_inliers_ = Config::get<int>("min_inliers");
key_frame_min_rot = Config::get<double>("keyframe_rotation");
key_frame_min_trans = Config::get<double>("keyframe_translation");
map_point_erase_ratio_ = Config::get<double>("map_point_erase_ratio");
orb_ = cv::ORB::create(num_of_features_, scale_factor_, level_pyramid_);
}
VisualOdometry::~VisualOdometry() {
}
//添加帧,同时进行
bool VisualOdometry::addFrame(Frame::Ptr frame) {
switch (state_) {
case INITIALIZING: {//只有第一帧会走这个case
//VO系统初始化部分,将第一帧位姿作为世界坐标系
state_ = OK;//修改 VO 状态 OK
curr_ = ref_ = frame;//第一帧既是当前帧也是前一帧
map_->insertKeyFrame(frame);//将第一帧作为关键帧
// extract features from first frame and add them into map
extractKeyPoints();//提取关键帧
computeDescriptors();//提取描述子
setRef3DPoints();//还原深度坐标
break;
}
case OK: {
curr_ = frame;//更新当前帧
extractKeyPoints();//提取关键帧
computeDescriptors();//提取描述子
featureMatching(); //特征值匹配
poseEstimationPnP(); //相机位姿估计PNP
if (checkEstimatedPose() == true) // a good estimation
{
curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_; // T_c_w = T_c_r*T_r_w
ref_ = curr_;//更新前一帧
setRef3DPoints();//还原深度
num_lost_ = 0;//没有丢失
if (checkKeyFrame() == true) // is a key-frame
{
addKeyFrame();
}
} else {// bad estimation due to various reasons
num_lost_++;
if (num_lost_ > max_num_lost_) {
state_ = LOST;
}
return false;
}
break;
}
case LOST: {
cout << "vo has lost." << endl;
break;
}
}
return true;
}
//提取特征点
void VisualOdometry::extractKeyPoints() {
boost::timer timer;
orb_->detect(curr_->color_, keypoints_curr_);
cout << "extract keypoints cost time: " << timer.elapsed() << endl;
}
//计算描述子
void VisualOdometry::computeDescriptors() {
boost::timer timer;
orb_->compute(curr_->color_, keypoints_curr_, descriptors_curr_);
cout << "descriptor computation cost time: " << timer.elapsed() << endl;
}
//特征匹配
void VisualOdometry::featureMatching() {
boost::timer timer;
vector <cv::DMatch> matches;
//通过前后两帧的描述子得到特征点匹配集合
matcher_flann_.match(descriptors_ref_, descriptors_curr_, matches);
// select the best matches
//找到 matches数组中最小距离,赋值给min_dist
float min_dis = std::min_element(
matches.begin(), matches.end(),
[](const cv::DMatch &m1, const cv::DMatch &m2) {
return m1.distance < m2.distance;
})->distance;
feature_matches_.clear();//清理上一帧用过的特征
//根据最小匹配距离筛选特征匹配中比较好的匹配点
for (cv::DMatch &m: matches) {
if (m.distance < max<float>(min_dis * match_ratio_, 30.0)) {
feature_matches_.push_back(m);
}
}
cout << "good matches: " << feature_matches_.size() << endl;
cout << "match cost time: " << timer.elapsed() << endl;
}
//由于PNP条件为前一帧的3D坐标 和当前帧的2D坐标 所以在当前帧变为前一帧时 需要还原深度信息
void VisualOdometry::setRef3DPoints() {
// select the features with depth measurements
pts_3d_ref_.clear();//前前帧的3D点
descriptors_ref_ = Mat();
//遍历还原所有特征点的深度
for (size_t i = 0; i < keypoints_curr_.size(); i++) {
//找到深度信息
double d = ref_->findDepth(keypoints_curr_[i]);
//深度为负说明特征点无效
if (d > 0) {
//构造像素坐标到相机坐标点的3D坐标
Vector3d p_cam = ref_->camera_->pixel2camera(
Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), d
);
//构造Point3f点按行放入前一帧3D点存储器中
pts_3d_ref_.push_back(cv::Point3f(p_cam(0, 0), p_cam(1, 0), p_cam(2, 0)));
//将描述子按行加入前一帧描述子容器
descriptors_ref_.push_back(descriptors_curr_.row(i));
}
}
}
//PNP
void VisualOdometry::poseEstimationPnP() {
// construct the 3d 2d observations
vector <cv::Point3f> pts3d;//将参考帧的3D
vector <cv::Point2f> pts2d;//当前帧的2d
//将参考帧的3D坐标和当前帧的2D坐标加入容器
for (cv::DMatch m: feature_matches_) {
pts3d.push_back(pts_3d_ref_[m.queryIdx]);
pts2d.push_back(keypoints_curr_[m.trainIdx].pt);
}
//内参矩阵
Mat K = (cv::Mat_<double>(3, 3) <<
ref_->camera_->fx_, 0, ref_->camera_->cx_,
0, ref_->camera_->fy_, ref_->camera_->cy_,
0, 0, 1
);
//rvec :前一帧坐标系到当前帧的相机坐标系的旋转矩阵
//tvec :前一帧坐标系到当前帧的相机坐标系的平移向量(矩阵形式)
//inlers : 内点数组
Mat rvec, tvec, inliers;
//pnp求解 可以看文档了解参数:https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d
cv::solvePnPRansac(pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers);
num_inliers_ = inliers.rows;
cout << "pnp inliers: " << num_inliers_ << endl;
//由pnp求解出来的rvec个tvex求出 T
T_c_r_estimated_ = SE3(
SO3(rvec.at<double>(0, 0), rvec.at<double>(1, 0), rvec.at<double>(2, 0)),
Vector3d(tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0))
);
// using bundle adjustment to optimize the pose
//BA优化位姿g2o 有关g2o使用教程参考:
typedef g2o::BlockSolver <g2o::BlockSolverTraits<6, 2>> Block;
typedef g2o::LinearSolverDense<Block::PoseMatrixType>()
LinearSolver;
//初始化
Block::LinearSolverType *linearSolver = new LinearSolver();//线性求解器
Block *solver_ptr = new Block(linearSolver);
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
//优化器对象SparseOptimizer
g2o::SparseOptimizer optimizer;
//将优化算法设置给SparseOptimizer
optimizer.setAlgorithm(solver);
//添加顶点
g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap();
pose->setId(0);
pose->setEstimate(g2o::SE3Quat(
T_c_r_estimated_.rotation_matrix(),
T_c_r_estimated_.translation()
));
optimizer.addVertex(pose);
//添加边(一个内点一条边)
for (int i = 0; i < inliers.rows; i++) {
int index = inliers.at<int>(i, 0);
// 3D -> 2D projection
//新建一条边第三种边
EdgeProjectXYZ2UVPoseOnly *edge = new EdgeProjectXYZ2UVPoseOnly();
//设置对应边的参数
edge->setId(i);//设置ID
edge->setVertex(0, pose);//设置边对应的优化顶点
edge->camera_ = curr_->camera_.get();//设置相机模型
edge->point_ = Vector3d(pts3d[index].x, pts3d[index].y, pts3d[index].z);//设置3D点
//设置边的测量值
edge->setMeasurement(Vector2d(pts2d[index].x, pts2d[index].y));
edge->setInformation(Eigen::Matrix2d::Identity());
optimizer.addEdge(edge);
}
//执行优化
optimizer.initializeOptimization();//初始化整个优化器
optimizer.optimize(10);//开始执行优化,迭代 10次
//将结果转成 T
T_c_r_estimated_ = SE3(
pose->estimate().rotation(),
pose->estimate().translation()
);
}
bool VisualOdometry::checkEstimatedPose() {
// check if the estimated pose is good
//
if (num_inliers_ < min_inliers_) {
cout << "reject because inlier is too small: " << num_inliers_ << endl;
return false;
}
// if the motion is too large, it is probably wrong
Sophus::Vector6d d = T_c_r_estimated_.log();
if (d.norm() > 5.0) {
cout << "reject because motion is too large: " << d.norm() << endl;
return false;
}
return true;
}
//T中 R或 t比较大都是关键帧
bool VisualOdometry::checkKeyFrame() {
Sophus::Vector6d d = T_c_r_estimated_.log();
Vector3d trans = d.head<3>();
Vector3d rot = d.tail<3>();
if (rot.norm() > key_frame_min_rot || trans.norm() > key_frame_min_trans)
return true;
return false;
}
void VisualOdometry::addKeyFrame() {
cout << "adding a key-frame" << endl;
map_->insertKeyFrame(curr_);
}
}
run_vo.cpp
// -------------- test the visual odometry -------------
#include <fstream>
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>
#include "myslam/config.h"
#include "myslam/visual_odometry.h"
#include "../include/myslam/camera.h"
int main(int argc, char **argv) {
if (argc != 2) {
cout << "usage: run_vo parameter_file" << endl;
return 1;
}
myslam::Config::setParameterFile(argv[1]);//加载配置文件default.yaml
myslam::VisualOdometry::Ptr vo(new myslam::VisualOdometry);//创建VO框架对象
string dataset_dir = myslam::Config::get<string>("dataset_dir");//数据集绝对路径
cout << "dataset: " << dataset_dir << endl;
ifstream fin(dataset_dir + "/associate.txt");
if (!fin) {
cout << "please generate the associate file called associate.txt!" << endl;
return 1;
}
vector <string> rgb_files, depth_files;
vector<double> rgb_times, depth_times;
while (!fin.eof()) {
string rgb_time, rgb_file, depth_time, depth_file;
fin >> rgb_time >> rgb_file >> depth_time >> depth_file;//从associate.txt中按顺序读取 rgb, depth
//以下四行把rgb,depth全部存入对应的vector容器
rgb_times.push_back(atof(rgb_time.c_str()));
depth_times.push_back(atof(depth_time.c_str()));
rgb_files.push_back(dataset_dir + "/" + rgb_file);//绝对路径
depth_files.push_back(dataset_dir + "/" + depth_file);//绝对路径
if (fin.good() == false)
break;
}
myslam::Camera::Ptr camera(new myslam::Camera);//创建相机对象,在config/default.txt中获取相机参数
// visualization
cv::viz::Viz3d vis("Visual Odometry");//生成3D图像可视化对象vis
cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);//设置世界坐标系,相机坐标系
cv::Point3d cam_pos(0, -1.0, -1.0), cam_focal_point(0, 0, 0), cam_y_dir(0, 1, 0);//设置相机的朝向(光轴朝向)
cv::Affine3d cam_pose = cv::viz::makeCameraPose(cam_pos, cam_focal_point, cam_y_dir);//设置相机的位置与朝向
vis.setViewerPose(cam_pose);//在vis中显示出来
world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);//设置世界坐标系渲染属性
camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0);//设置相机坐标系渲染属性
//显示世界、相机部件
vis.showWidget("World", world_coor);
vis.showWidget("Camera", camera_coor);
//rgb文件数量
cout << "read total " << rgb_files.size() << " entries" << endl;
//读取所有rgb文件
for (int i = 0; i < rgb_files.size(); i++) {
//将rgb图像数据转化为 rgb图像矩阵
Mat color = cv::imread(rgb_files[i]);
//将深度图数据转化为深度矩阵
Mat depth = cv::imread(depth_files[i], -1);
if (color.data == nullptr || depth.data == nullptr)
break;
//生成帧对象,将遍历到的图像信息存放到帧里面去
myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();
pFrame->camera_ = camera;//相机对象早在生成时就初始化好了
pFrame->color_ = color;
pFrame->depth_ = depth;
pFrame->time_stamp_ = rgb_times[i];
//将当前帧加入VO系统
boost::timer timer;
vo->addFrame(pFrame);
cout << "VO costs time: " << timer.elapsed() << endl;
//如果帧丢失直接结束
if (vo->state_ == myslam::VisualOdometry::LOST)
break;
//这里视第一帧的相机坐标系为世界坐标系,现在求出的是世界坐标系到相机坐标系的变换矩阵
SE3 Tcw = pFrame->T_c_w_.inverse();
// show the map and the camera pose
//求出世界坐标系到相机坐标系的欧式变换矩阵
cv::Affine3d M(
cv::Affine3d::Mat3(
Tcw.rotation_matrix()(0, 0), Tcw.rotation_matrix()(0, 1), Tcw.rotation_matrix()(0, 2),
Tcw.rotation_matrix()(1, 0), Tcw.rotation_matrix()(1, 1), Tcw.rotation_matrix()(1, 2),
Tcw.rotation_matrix()(2, 0), Tcw.rotation_matrix()(2, 1), Tcw.rotation_matrix()(2, 2)
),
cv::Affine3d::Vec3(
Tcw.translation()(0, 0), Tcw.translation()(1, 0), Tcw.translation()(2, 0)
)
);
cv::imshow("image", color);
cv::waitKey(1);
vis.setWidgetPose("Camera", M);
vis.spinOnce(1, false);
}
return 0;
}
二、搭建:
请确保在看完0.2版本之后再开始以下内容
编译工程
在0.3目录下打开终端:
mkdir build
cd build
cmake ..
make
出现以下错误
In file included from /usr/local/include/g2o/core/base_unary_edge.h:30:0,
from /home/zqh/1/slambook-master/project/0.3/include/myslam/g2o_types.h:27,
from /home/zqh/1/slambook-master/project/0.3/src/g2o_types.cpp:1:
/usr/local/include/g2o/core/base_fixed_sized_edge.h:200:32: error: ‘index_sequence’ is not a member of ‘std’
....
原因分析:
slambook1版本g2o 版本太老了 使用的是c++11版本的g2o,我用的是最新的g2o,使用的是C++14标准
解决:
让CMake编译在C++14标准下进行,在CMakeLists.txt添加:
set(CMAKE_CXX_STANDARD 14)
修改完后重新编译出现:
In file included from /home/zqh/1/slambook-master/project/0.3/src/g2o_types.cpp:1:0:
/home/zqh/1/slambook-master/project/0.3/include/myslam/g2o_types.h:37:80: error: ‘VertexSBAPointXYZ’ is not a member of ‘g2o’
rojectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>
^~~~~~~~~~~~~~~~~
/home/zqh/1/slambook-master/project/0.3/include/myslam/g2o_types.h:37:80: note: suggested alternative: ‘VertexPointXYZ’
rojectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>
^~~~~~~~~~~~~~~~~
VertexPointXYZ
...
原因分析:
新版g2o库中不再存在原先的VertexSBAPointXYZ,将所有报错的地方改为VertexPointXYZ即可