vo类总结

1.Camera类

camera类里面，首先camera有5个变量，fx_,fy_,cx_,cy_,depth_scale_5个变量,由外部传fx,fy,cx,cy,depth_scale给它。
定义了一个智能指针，如今还不知道这个智能指针有什么用
typedef std::shared_ptr<Camera> Ptr,之后传递参数时使用Camera::Ptr就能够了。
camera.h文件里定义6个函数的声明。包括相机坐标系和世界坐标系之间的转换，相机坐标和像素坐标，世界坐标和像素坐标。世界坐标和像素坐标之间的转换是先把它转成中间项相机坐标，再转的。
位姿都是T_c_w.若是是相机转世界坐标系，用T_c_w的逆矩阵就能够了。
像素坐标和相机坐标之间转，定义了一个depth,z坐标直接是这个depth.

2.Frame类
变量有6个，id,时间戳，T_c_w,Camera类的智能指针，彩色图和深度图，
由于用到了Camera::Ptr,因此要#include "../myslam/camera.h"
id,时间戳time_stamp是用来记录帧信息的，T_c_w帧计算时会用到，好比计算帧的像素坐标，而Camera::Ptr用这个指针能够访问类Camera里的任意变量和函数，能够任意使用它的函数。直接camera->就能够访问了。彩色图和深度图，是用来获得深度和关键点的。
里面有3个函数，一个是
（1）createFrame(),没有变量，里面只是定义factory_id=0,而后返回Frame::Ptr(new Frame(factory_id++)
彷佛返回的是一个新的指针。
（2）findDepth是用来发现关键点的深度的，返回值是double类型，变量是const cv::KeyPoint& kp
这个函数首先获得kp的像素坐标x,y.
int x=cvRound(kp.pt.x)
int y=cvRound(kp.pt.y)
而后在深度图上读取ushort d.若是d!=0，就能够返回深度double(d)/camera_->depth_scale_;
这里的深度是要除以深度尺度因子的。
若是读到的深度为0，那么依次用(y)[x-1],(y-1)[x],(y)[x+1],(y+1)[x]来代替深度d,若是d不为0，返回double(d)/camera_->depth_scale_;
默认返回-1.0;
（3）函数计算相机光心坐标，类型Vector3d。没有变量，直接是T_c_w_.inverse().translation();
(4)知道点的世界坐标系坐标，判断这个点是否在帧上，返回bool类型。
变量就是const Vecotr3d& pt_world)
首先计算pt_cam,若是它的z坐标小于0，就返回false.
再计算pt_pixel,若是它的像素坐标知足0<u<color_.cols,0<v<color_.rows，则返回true.
3MapPoint类
MapPoint是路标点，会提取当前帧的特征点，而后与它匹配，来估计相机的运动。由于要计算匹配，因此还要存储路标点的描述子。
还要记录路标点被观测到的次数和被匹配的次数，做为评价其好坏程度的指标。
差很少6个变量吧，路标点id,世界坐标系坐标，norm好像是观测的方向，它的描述子，不过它的类函数中没有描述子。只有id,pos,norm,还把观测次数和被匹配次数初始化为0.
并且没有传变量的时候，把id赋值为-1，pos,norm,observed_times,correct_times都设为0.
里面只有一个createMapPoint函数，定义factory_id=0,返回值类型是MapPoint的智能指针。返回的是new MapPoint(factory_id++,Vector3d(0,0,0),Vector3d(0,0,0).
4Map类
Map管理的是路标点和关键帧。VO的匹配过程只须要和Map打交道。
Map做为类，只定义了两个量，一个是全部的路标点
unordered_map<unsigned long,MapPoint::Ptr> map_points_;
一个是全部的关键帧
unordered_map<unsigned long,Frame::Ptr> keyframes_;
由于用到了路标点和关键帧，全部要include map.h和mappoint.h.
也只定义了两个函数，一个是插入关键帧 insertKeyFrame
先输出当前关键帧的数量。
而后判断要插入的帧frame的id_是否是和keyframes_.end()相等，若是相等，说明须要插入，把frame的id号和frame一块儿打包插入
keyframes._.insert(make_pair(frame->id_,frame));
插入路标点的函数同理。
map类是经过散列Hash来存储的，方便随机访问和插入、删除。
5.config类
这个类主要作的是文件的读取。并且在程序中能够随时提供参数的值。这里把config类写成单件(singleton)模式，它只有一个全局对象，当咱们设置参数文件的时候，建立该对象并读取参数文件，随后就能够在任意地方访问参数值，最后在程序结束时自动销毁。

Config类负责参数文件的读取，并在程序的任意地方均可随时提供参数的值。因此咱们把config写成单件(singleton)模式，它只有一个全局对象，当咱们设置参数文件时，建立该对象并读取参数文件，随后就能够在任意地方访问参数值，最后在程序结束时自动销毁。自动销毁？
把指针定义成config_,为何不定义成Ptr了
咱们把构造函数声明为私有，防止这个类的对象在别处创建，它只能在setParameterFile时构造，实际构造的对象是config的智能指针：static std::shared_ptr<Config> config_.用智能指针的缘由是能够自动析构，免得咱们再调一个别的函数来作析构。
2.在文件读取方面，咱们使用OpenCV提供的FileStorage类，它能够读取一个YAML文件，且能够访问其中任意一个字段，因为参数实质值可能为整数、浮点数或字符串，因此咱们经过一个模板函数get来得到任意类型的参数值。
模板其实就是一个类型啊。
下面是config的实现，注意，咱们把单例模式的全局指针定义在此源文件中了。
在实现中，咱们只要判断一下参数文件是否存在便可。定义了这个Config类以后，咱们能够在任何地方获取参数文件里的参数，例如，当想要定义相机的焦距fx时，按照以下步骤操做便可：
1.在参数文件中加入”Camera.fx:500"
2.在代码中使用：
myslam::Config::setParameterFile("parameter.yaml");
double fx=myslam::Config::get<double>("Camera.fx");
就能得到fx的值了。
固然，参数文件的实现不仅这一种，咱们主要从程序开发的便利角度来考虑这个实现，读者固然也能够用更简单的方式来实现参数的配置。
把智能指针定义成了config_,还有一个file_
两个函数，一个是setParameterFile,变量是文件名。
一个是get函数，返回类型根据get<>后面参数决定，返回值是config_->file_[key]
setParameterFile,若是config_是空指针，就
config_=shared_ptr<Config>(new Config)
就为新的Config指针？
config_->file_就定义为cv::FileStorage形式，变量是filename.c_str(),参数是cv::Filestorage::READ.是只读模式，不修改。
若是文件打不开，就用std::cerr输出错误信息。
发布release.
而~Config()函数，里面判断了file_,若是file_可以打开，就发布file_
6.VisualOdometry类
6.1变量
首先它也有一个智能指针，这样以后以后就能够经过定义VisualOdometry::Ptr vo;而后经过vo->来访问它的变量还有函数了。先来看它的变量。
6.1.1vo的状态变量state_
它总共就三个值INITIALIZING=0,OK=1,LOST,以后会根据vo的不一样状态进行不一样的计算。
6.1.2用于计算的变量
map_,ref_,cur_,orb_
地图是一个总变量，用来看帧是否是关键帧，若是是，就把它添加到地图里。还能够输出地图里关键帧的总数。至于ref_,cur_就是用来计算的两个帧，计算它两个之间的T_c_r_estimated_,orb_是个提取orb特征的智能指针，把当前帧的彩色图放进去，能够计算当前帧的关键点和描述子。
6.1.3计算出来的中间变量
pts_3d_ref_,这个是参考帧的特征点的世界坐标系的坐标，每次换参考帧，它都得更改一下。
keypoints_curr_,当前帧的关键点，也能够说是特征点。
descriptors_ref_,descriptors_curr_,pts_3d_ref,和descriptors_ref是一块儿计算的，这两个描述子是用来计算匹配的，匹配通过筛选以后变成feature_matches_.
根据pts_3d_ref_,由当前帧的像素坐标有pts_2d,由pnp能够计算出T_c_r_estimated_.
num_inliers_,pnp函数返回值有一个inliers,num_inliers_=inliers.rows,用来和min_inliers作比较的。和T_c_r_estimated.log()的范数一块儿，用来检测位姿的。
num_lost_,若是位姿估计不经过的话，num_lost_就会加1，若是num_lost_>max_lost_，vo的状态就会变成LOST.
6.1.4阈值，用来作比较的值
match_ratio_,用来筛选匹配的，若是匹配距离小于(min_dist*match_ratio_,30.0)，匹配是好匹配。
max_num_lost_,用来判断vo状态，若是位姿检测不经过数达到必定值，vo的值定为LOST.
min_liners_,若是有效特征点数num_inliers小于min_liners,那么位姿估计不经过。
key_frame_min_rot_,最小旋转，用来检测关键帧，当前帧和参考帧的变换的范数只有大于最小旋转或最小位移，当前帧才是关键帧。也就是关键帧检验才经过。
key_frame_min_trans_,最小位移。
6.1.5建立orb_的变量。
num_of_features_,应该每一个帧提取的特征数应该是固定的。
scale_factor_,图像金字塔的尺度因子，应该提取图像特征的时候会用到。
level_pyramid_图像金子塔的层级。
把这三个变量值放进cv::ORB::create()函数里就能够直接建立orb_了。
orb_=cv::ORB::create(num_of_features_,scale_factor_,level_pyramid_).
阈值和建立orb_的三个变量值都是经过读取config类的参数文件获得的。直接Config::get<参数类型>("参数文件名")
参数类型，带num的都是整数，金字塔层级也是整数，匹配比例float,最小位移，最小旋转和尺度比例都是double类型。

6.2函数
它有8个函数。来看每一个函数的具体实现。
6.2.1extractKeyPoints()
返回值为空，没有变量。
做用，提取当前帧的彩色图的关键点，获得keypoints_curr_.
6.2.2computeDescriptors()
返回值为空，没有变量。
做用：计算当前帧的特征点的描述子，获得descriptors_curr_.
6.2.3featureMatching
返回值为空，没有变量。
做用：计算参考帧和当前帧之间的匹配，获得筛选后的匹配feature_matches,输出筛选后的匹配数。
过程：计算参考帧和当前帧之间的匹配，返回索引和匹配距离。用的是cv::BFMatcher,距离用的是汉明距离NORM_HAMMING,求最小距离用的是std::min_element,筛选匹配用的是min_dis*match_ratio_,30.0,筛选以后的匹配放入feature_matches_.
6.2.4setRef3DPoints()
返回值为空，没有变量。
做用：获得参考帧的特征点的相机坐标和参考帧的描述子矩阵。
过程：制做一个for循环，读取参考帧上每一个特征点的深度值，ref_是帧，帧类有发现深度函数findDepth.
double d=ref->findDepth(keypoints_curr[i].pt),只因此这里用的是keypoints_curr_,是由于参考帧仍是当前帧，计算的时候。ref_=curr_.
若是深度大于0，根据帧类有cmera_指针，cmera类有各类坐标转换函数。能够把参考帧的像素坐标加深度转成相机坐标。
把相机坐标都放到pts_3d_ref_.
再计算一下描述子矩阵。
6.2.5poseEstimationPnP()
返回值为空，没有变量。
做用：获得有效特征点数num_inliers和当前帧和参考帧之间的位姿估计值T_c_r_estimated_
过程,输出有效特征点数
定义pts3d,pts2d,一个放的是参考帧的相机坐标值，一个放的是当前帧的特征点的像素坐标值。
定义相机内参矩阵K.至于里面的fx_,fy_,cx_,cy_都在帧类的cmera类的变量值里有定义。
使用函数cv::solvePnPRansac(pts3d,pts2d,K,Mat(),r_vec,t_vec,false,100,4.0,0.99,inliers)
rvec,tvec,inliers都是返回值，false是不用外参，100是迭代次数，4.0是重投影偏差，0,99是可信度。
由inliers获得num_inliers_.num_inliers_=inliers.rows.
由rvec,tvec获得T_c_r_estimated_，是SE3,李代数形式。
输出有效特征点数。
6.2.6checkEstimationPose()
返回bool值，没有变量。
做用：对估计出来的位姿进行检测，若是有效特征点数太少或者运动太大，检测不经过。
过程：若是num_inliers<min_inliers_,那输出拒绝由于有效特征点数太少,并输出有效特征点数。
定义d为位姿估计值的对数形式。
Sophus::Vector6d d=T_c_r_estimated_.log()
若是d.norm()>5.0,那么输出拒绝由于运动太大，并输出d.norm().
6.2.7checkKeyFrame()
返回true或false,没有变量。
做用：检测当前帧是否是关键帧。若是当前帧相对于参考帧移动的位移大于最小位移或旋转大于最小旋转，能够把当前帧当作关键帧加如地图。
过程：
一样定义d为估计值的对数形式。d是位移在前，旋转在后
trans取d的前3个值，rot取d的后3个值，若是trans.norm()>key_frame_min_trans_或者rot.norm()>key_frame_min_rot_，就说明检测经过。
6.2.8addKeyFrame()
返回值为空，没有变量。
做用：用于把当前帧添加到地图里，并输出加入当前帧的提示。
6.2.9总函数addFrame()
添加帧函数。
返回true或false,变量是Frame::Ptr frame.
做用：对输入的帧作处理，同时地图，vo的状态值作相应的变换。
过程：
是一个case.用来判断vo的状态值state_,根据state_的不一样值作不一样的处理。
当state_为INITIALIZING的时候，状态值变为OK，参考帧当前帧都为输入帧，地图把输入帧之间加入地图，不作检测。而后对输入帧提取关键点，计算描述子，此时输入帧已是参考帧了，设置它的特征点的相机坐标和描述子。依次用的函数是extractKeyPoints();computeDescriptors();setRef3DPoints().
当state_为OK的时候，输入帧被当作是当前帧，而后对当前帧提取关键点，计算描述子，和参考帧的描述子进行匹配并筛选匹配，而后用参考帧的相机坐标和当前帧的像素坐标求解pnp,获得T_c_r_estimated_.依次用的函数是 extractKeyPoints();computeDescriptors(); featureMatching();
poseEstimationPnP();而后对位姿估计值进行检测，若是检测经过，计算T_c_w_,而后当前帧就变成了参考帧，设置新一轮的参考帧的相机坐标和描述子。用的函数是checkEstimationPose(),setRefPoints().
而后进行关键帧检测，若是关键帧检测经过，地图上把当前帧给加进去。用的函数checkKeyFrame(),addKeyFrame().
若是位姿估计值检验没有经过的话，num_lost_+1,若是num_lost_>max_num_lost_,就把state_状态值设为LOST.返回False.
当state_状态值为LOST的时候，输出vo状态丢失。

7.g2o_types.h类

为了避免跟以前的重合，实际定义时
定义成#ifndef MYSLAM_G2O_TYPES_H来着。
一个h文件能够定义多个类。里面就定义了3个类。由于都是边类，因此都是以EdgeProjectXYZ开头。
若是只是RGBD的话，名字EdgeProjectXYZRGBD,3d-3d,继承自二元边，3,测量值类型Eigen::Vector3d,两个顶点，一个是g2o::VertexSBAPointXYZ,一个是g2o::VertexSE3Expmap，就是位姿。
若是RGBD只考虑位姿，名字EdgeProjectXYZRGBDPoseOnly，继承自一元边，2，测量值Vector3d,顶点类型g2o::VertexSE3Expmap.
若是不用RGBD，就是3d-2d的话。名字EdgeProjectXYZ2UVPoseOnly,继承自一元边，测量值Vector2d,就是像素坐标，顶点类型g2o::VertexSE3Expmap,就是位姿了。
里面基本上都是先EIGEN_MAKE什么，而后声明一下四个虚函数，分别是计算偏差函数computeError(),求雅克比矩阵函数linearlizeOplus(),读写函数read,write.读写返回bool,前两个返回空。
形式以下：
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void computeError();
virtual void linearizeOplus();

virtual bool read(std::istream& in){}
virtual bool write(std::ostream& os)const {}
只是类EdgeProjectRGBDPoseOnly多了一个变量point_,EdgeProjectXYZ2UVPoseOnly由于要计算像素坐标还多了一个camera_.
须要咱们写就两个函数computeError()和linearlizeOplus()函数。
（1）computeError(）函数
第一步都是定义顶点，而后根据顶点的估计值算出估计值，而后和测量值一块儿算偏差。
g2o::VertexSE3Expmap* pose= static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
这里没有new.它是把_vertices[0]赋值给顶点，就是位姿。而后根据位姿的估计值对点进行映射成相机坐标再算出像素坐标值，和测量值进行比较就获得偏差了。
_error=_measurement-camera_->camera2pixel(pose->estimate().map(point_));
也是由于须要用到camera_,point_，因此才定义了这两个变量。
（2）linearizeOplus函数
仍是先把_vertices[0]赋值给pose.前后把T定义为pose的估计值，而后把xyz_trans定义为T.map(point_).
而后xyz_trans[0],xyz_trans[1],xyz_trans[2]分别为x,y,z，用来计算雅克比矩阵。
雅克比矩阵为像素u对空间点P求导再乘以P对yi*李代数求导。
前一个2*3,后一个3*6,为[I,-空间点的反对称矩阵】实际计算的时候把空间点的反对称矩阵放在前面，并且求负。【空间点的反对称矩阵，-I】