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】