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游戏。