vo优化总结

问题1：
位姿估计用的ransac,只用了几个点，若是3d_2d点存在噪声，不行。
优化：把这值当作初值，用非线性优化
问题2：
深度图有偏差，深度过近或过远不行，有偏差。而特征点每每在物体边缘处，深度测量值一般不许。因此直接把特征点的3D位置当作真值来作不行。必须对特征点进行优化。
问题3：
两两帧不行，参考帧质量差（严重遮挡，光照变化），丢失。参考帧位姿估计不许（漂移）。
优化：当前帧与地图进行匹配。
问题4：
运行时间特征提取10，描述子计算9，特征匹配10，pnp求解1，其余0.1,总计30.
优化：提升特征提取，匹配的速度。
优化2：不特征匹配，直接法，或光流法。
只通过内参，和depth_scale，只是像素坐标和相机坐标的转换，而不是世界坐标，世界坐标要有T_c+w.
向量才有size().
一.针对pnp的优化
就是把ransac算出来的值当作初值，而后用非线性优化来优化。有一个好的初值对计算也是挺有用的。像以前的直接法里把Tcw的初值设为单位矩阵，而后pose设置估计值的时候就把Tcw给放上去了。
Eigen::Isometry3d Tcw=Eigen::Isometry3d::Identity();
pose->setEstimate(g2o::SE3Quat(Tcw.rotation(),Tcw.translation()));
这里也是SE3 T_c_r_estimated_，
pose->setEstimate(g2o::SE3Quat(T_c_r_estimated_.rotation_matrix(),
T_c_r_estimated_.translation()));
就是Eigen::Isometry3d和SE3仍是有一点不同的，一个.rotation(),一个.rotation_matrix()。
这里的T_c_r_estimated_是由cv::solvePnPRansac获得rvec,tvec，而后rvec.at变成SO3形式，tvec.at变成Vector3d形式一块儿组成SE3格式的T_c_r_estimated_.
设置g2o_types类，里面定义所须要用到的边类。EdgeProjectXYZ2UVPoseOnly.
都是同样的套路，先typedef Block,而后linearSolver,solver_ptr,solver,而后设置图模型optimizer,optimizer.setAlgorithm(solver).
就是设置顶点的时候，顶点的估计值变成T_c_r_estimated_.
设置for循环时，循环数由inliers.rows.
index被定义为int index=inliers.at<int>(i,0).
因为在边类中设置了两个变量。这里须要给值。
edge->camera_=curr_->camera.get().
edge->point_就是由pts3d[index].x,.y,.z组成的，测量值就是当前帧的像素坐标。
因为测量值2维，信息矩阵是2*2的单位矩阵。
二.针对地图的优化
不是两个两个的了。而是当前帧与局部地图的描述子的匹配。里面牵涉到对路标点的处理。若是初始化状态，把当前帧的全部特征点都当作路标点加入到地图。那每次匹配以前，从当前地图路标点中挑选可被当前帧可见的路标点，取这些路标点的描述子与当前帧进行匹配并筛选匹配。
求解以后对地图进行优化，其实就是对路标点，若是不在当前帧可见范围内，匹配比例小于阈值的，观测角大于pi/6的路标点要删除，筛选匹配后当前帧的特征点小于100的话，增长路标点，若是路标点数量大于1000，删除比例加0.05.
详情见改进地图的vo类.note

如今的地图只是各帧特征点的集合。
建立地图：局部，全局。
局部：只相机位置附近的特征点，用来和当前帧匹配求解相机位置的。
全局：不定位，回环检测和地图表达。
重点或麻烦：维护局部地图的规模。为了实时，保证地图规模不能太大。
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已丢失。
3.建议
关键帧的处理，这主要是后端的事了。主要要保证相机在原地不动的时候，地图和轨迹不会变大。
常见：取过必定时间间隔的帧作关键帧，关键帧会被仔细优化，而中间的就忽略了。
看图：若是某个特征点不规则的运动，说明估计不许或特征点的位置不许。
4缺陷

 

没有提供对地图的优化，能够优化，方向最小二乘和三角化。的确，对点的话。
特征点的vo大约可以实时处理多少个ORB特征点。
可以估算局部时间内的相机运动及特征点的位置，局部缺点：
1.容易丢失，一旦丢失等待相机转回来（保存参考帧并与新的帧进行比较），要么重置vo
2.漂移，偏差的累积。大一点的局部地图能够缓解。
若是只关心短期内的运动，不用彻底的slam,只用vo就能够了。好比无人机控制和ar游戏。