改进地图的vo类

现在的地图只是各帧特征点的集合。
创建地图：局部，全局。
局部：只相机位置附近的特征点，用来和当前帧匹配求解相机位置的。
全局：不定位，回环检测和地图表达。
重点或麻烦：维护局部地图的规模。为了实时，保证地图规模不能太大。
1.修改地图点类MapPoint
增加变量和函数形式。
1.1类
变量倒也简单。两个id,一个id_,一个factory_id_,主要用的是factory_id.
向量形式的pos_,norm_,一个是路标点的世界坐标，一个是路标点被观测到的方向。
good_,判断路标点是好是坏的指标。
descriptor_路标点的描述子，因为要用路标点的描述子和当前帧的描述子进行匹配。
次数matched_times visible_times_,在位姿估计中成为内点的次数和在当前帧中可见的次数。
还有帧列表observed_frames_,可以观测到路标点的关键帧。
类中还有一个内联函数getPointCV,它是用来返回pos_的Point3f形式。
1.2类函数
MapPoint函数的两种形式，主要用于new MapPoint()赋值。迭代的每一次赋值。
一个是没有变量，把id赋值为-1，次数都为0，good_是true,pos_,norm为零向量。这应该是初始化的时候。
一个主要作用是观测帧列表中加入新的帧。变量有id,pos_,norm,descriptor_,frame_，这和下面的createMapPoint相呼应。
createMapPoint函数的两种形式。它主要返回MapPoint的指针。一个也是没有变量，返回的是new MapPoint(factory_id++,Vector3d(0,0,0),Vector3d(0,0,0));
一个有前面的几个变量，返回的是MapPoint(factory_id_++,pos_world,norm,frame,descriptor);
还定义了factor_id_初值为0.
2.visualOdometry类的变化
2.1增加的变量
cv::FlannBasedMatcher matcher_flann_，这是匹配子，它的match函数可以用来做desp_map和当前帧的描述子来做匹配。
vector<MapPoint::Ptr> match_3dpts_，这是有MapPoint的智能指针组成的向量，经过两次选择，一次是从地图中的路标点中选，选世界坐标转像素坐标在当前帧范围内的。这里的T_c_w用的是参考帧的。第二次是把desp_map和当前帧的描述子进行匹配，选择匹配距离小于mis_dis*match*ratio和30的描述子对应的路标点。
vector<int> match_2dkp_index_筛选匹配后当前帧的特征点的索引。
T_c_w_estimated_一起估计的都是当前帧和参考帧之间的，所以用的是相机坐标，这里用世界坐标，计算的是T_c_w的孤寂值。
阈值map_point_erase_ratio_，应该是路标点的匹配比例，如果solvePostiationPnP中有用到的话，路标点匹配次数会加1，如果路标点转成像素坐标后在当前帧里，观测次数加1.匹配次数除以观测次数得到匹配比例，这个值默认为0.1,如果匹配比例小于它，会把该路标点从当前地图中删除。
2.2修改的函数
2.2.1特征匹配函数featureMatching()
作用：构建求解相机位姿的局部地图的特征点的世界坐标match_3dpts_，经过cv::Point3f变过之后就是pts3d和当前帧的像素坐标的索引match_2dkp_index_.
过程：(1)构建局部地图的描述子矩阵desp_map，和存放可见路标点指针的矩阵candidate.
对于地图中的所有路标点都做一个判断，如果路标点的世界坐标经过变换得到的像素坐标在当前帧上，即if(curr_.isInFrame(p->pos_))，那么这个路标点的可见次数加1，candidate存放这个路标点，desp_map存放这个路标点的描述子。
（2）对desp_map和当前帧的描述子矩阵用flann法进行匹配，得到matches,然后得到matches的最小距离min_dis.筛选匹配，假设通过的匹配是m,那么match_3dpts_路标向量存放candidate里通过匹配的路标，match_2dkp_index_里存放通过匹配的当前帧的特征点的索引。
2.2.2位姿估计函数poseEstimationPnP()
作用：求解Ｔ_c_w_estimated并进行g2o优化。
过程：得到pts3d,pts2d,K,等，然后用cv::solvePnPRansac函数求解得到rvec,tvec,inliers.然后得到T_c_w_esitmated_,然后把它当做初值进行优化，优化边和位姿跟之前一样，在添加一条条边的时候，真正用于匹配的特征点的索引会存放在inliers中，可以通过inliers.at<int> ( i,0 )得到。这样用于匹配的路标点的匹配次数就会加1.
2.2.3添加关键帧函数addKeyFrame
作用：一般是在判断对当前帧进行检测之后，如果检测通过，添加关键帧。这里多加了一步，就是在初始化的时候，把当前帧的所有特征点都当做路标点加入到地图。
过程：（1）如果地图里的关键帧为空的话map_->keyFrame.empty()，做一个for循环,根据当前帧的关键点的大小keypoints_curr_.size(),得到d,如果d<0,跳出循环，由d得到p_world,是根据参考帧里cmaera里的函数，用的却是当前帧的T_c_w.p_world和参考帧的相机中心相减得到n.
n.normaiize(）,然后p_world,规范化之后的n，就是观测方向，当前帧的描述子的clone,当前帧，组合得到路标指针map_point,.
用地图类中的insertMapPoint函数把这些路标点插入到地图中。
(2)否则地图插入关键帧，参考帧变成当前帧。
2.3增加的函数
2.3.1添加路标点函数addMapPoints
作用：往地图上添加路标点，是当前帧和他可见的路标点进行匹配，筛选匹配后的特征点小于100的时候，添加路标点。而且添加的是当前帧的特征点。
过程：造一个跟当前帧的关键点数量相同的bool向量matched，先全赋值为false.
对于匹配中用到的当前帧的特征点，对应的索引在matched设为true.
造一个for循环，根据当前帧的关键点的数量来定。
对于matched==true的那些i，不做处理。就是匹配中用到的那些特征点，不做处理。
其他的特征点，求d,然后p_world,然后n,n.normalize,map_point,然后地图把这里路标点给插入进去。
2.3.2优化地图函数optimizeMap()
作用：移除当前帧已经看不见的路标点和匹配比例不好的路标点
过程：（1）做一个for循环，所有路标点进入循环，iter代表地图中的一个路标点，但iter.second才是路标点的值。如果路标点的位姿经过转换之后不在当前帧范围之内，删除路标点。
计算匹配比例，就是匹配次数除以可见次数，如果比例小于阈值map_point_erase_ratio_,删除此路标点。
删除是iter=map_->map_points_.erase(iter);
计算观测角angle,根据当前帧和路标点位姿计算到的，就是路标点的位姿减去当前帧的相机中心得到n,n规范化之后，n.transpose()*point->norm_,就是n的逆乘以路标点的观测方向再经过acos变换得到的角度。
如果这个角度大于M_Pi/6,那么就删除此路标点。
（2）如果筛选匹配后的当前帧的特征点小于100，执行添加路标点操作，供下一次使用。
如果路标点的数量大于1000，那么map_point_erase_ratio_加上0.05.如果不大于1000，就还是0.1.
2.3.3得到观测角函数getViewAngle()
作用：变量是当前帧和路标点。路标点的世界坐标减去当前帧的相机中心得到n.n经normalize之后，
acos(n.transpose()*point->norm_)就得到了观测角。
观测角主要在优化地图的时候用到，如果观测角大于pi/6,说明已经快看不见路标点 了，把路标点删除。
2.4总函数addFrame()
输入变量为帧类
过程：初始化的时候，对输入帧提取关键点，计算描述子和添加关键帧，此时，输入帧即为参考帧，又为当前帧。
状态ok的时候，输入帧为当前帧，把参考帧的T_c_w当做当前帧的T_c_w的初值。提取当前帧的额关键点，描述子，把地图上路标点的描述子和当前帧的描述子进行匹配，desp_map只可能小于当前帧的描述子数量，不可能大于。并筛选匹配。然后根据匹配结果得到pts3d,pts2d，根据pnp求解出当前帧的T_c_w_estimated.
如果位姿估计检验通过，就是内点数不能太少，运动不能太大。当前帧的T_c_w就等于它的估计值。执行优化地图操作，就是对不在当前帧可见范围内，匹配比例过小，观测角度过大的路标点进行删除。如果筛选匹配后当前帧的特征点过小，添加路标点，如果总路标点数量太大，增加删除比例。
如果关键帧检验通过的话，就是旋转或位移有一个大于最小值的话，添加关键帧。
如果关键帧检验没有通过，num_lost+1,如果num_lost大于阈值，vo状态设为丢失。、
如果为丢失，跳出循环。输出vo已丢失。