主要是frame_handler_mono.cpp
关于ros部分不了解.
做的笔记主要来方便保存,不定正确....
主要目的还是想代码 论文结合的整明白.
initialize:
1.快速的特征检测:返回相机分辨率的宽度,高度,格子数,金字塔层数2.边的检测:相同
3.这是一个绑定函数.(对boost不了解)
4.深度滤波线程
1.添加一张图像.
2.判断启动程序是否启动.
3.清理一些离的更近的关键帧.
4.清理所有具有重叠视野的关键帧.
5.创建新的帧
5.判断帧的类型并根据类型进行相应的操作.
6.如果当前状态判断为第二帧
6.1返回第一二关键帧由单映性估计出的Pose的参考帧keypoints坐标集
6.2返回第一二关键帧由单映性估计出的Pose的当前帧被追踪的keypoints坐标集
6.3返回第一二关键帧由单映性估计出的Pose的特征点类型
6.4迭代器的遍历 遍历当前帧和参考帧的特征点
6.5如果type为1(代表什么?) 画红线 否则画绿线
7.如果当前状态不是第二帧
7.1 判断是否为关键帧 如果是,变为蓝色 如果不是 为绿色
7.2 迭代器的遍历 遍历此帧
7.3 判断此帧特征类型为边还是点
8.显示 end debug
9.将新来的帧赋予last_frame_
10.判断新来帧为关键帧并且将已将此帧赋予last_frame_ 则将新来帧重置.
11.调用finishFrameProcessingCommon成员函数
11.1
processFirstFrame()
1.新建一个新来帧的变换矩阵T_f_w_表示从世界坐标到相机?(Transform (f)rame from (w)orld)2.对new_frame_新来帧调用初始化类中的addFirstFrame成员函数判断是否为initialization::FAILURE,如果是则结束,并返回RESULT_NO_KEYFRAME.
2.1addFirstFrame成员函数 先进行reset()成员函数,清理参考帧keypoints点集,重置参考帧.
2.2 调用detectFeatures成员函数 对输入的帧 也就是new_frame_帧进行Fast特征检测.输出为对应特征点位置和单位方向向量
2.2.1 调用feature_detection::FastDetector detector, FastDetector又包含了抽象检测AbstractDetector主要是将图像分为格子,将特征分散
2.2.2detector.detect 对每层金字塔都进行fast特征检测选择一些得分高的角点放入new_features.
2.2.3 与2.2.1类似 变成对特征类型为边而已
2.2.4 与2.2.2类似
2.2.5 对new_features遍历 返回特征位置和对应的特征单位方向向量
3.经过第二步,如果继续,就可以判断此帧为关键帧,设为关键帧.
3.1 点开setKeyframe,里面是setKeyPoints (这些点用于快速检测是否两个帧有重叠的视野，选取5个特征，一个在图像中点另外4个靠近图像的4个边角，并且这5个特征都要有对应的3D点)
3.2 再点开setKeyPoints,如果特征指向的3D点为空,则设置该特征为NULL.再通过调用checkKeyPoints函数,对当前图像中每个特征点进行遍历比较,最终选出最具有代表性的5个作为关键点。实质上是1个靠近图像中心的点和4个靠近图像四个角的点.
4.不知道为啥我的addKeyframe点不进去...
5.stage_设为second_frame
6.将信息"Init: Selected first frame."记录在日志
7.返回 result_is_keyframe.
processSecondFrame
1.调用initialization.cpp中addSecondFrame函数返回一个初始化结果.
1.1调用tracklt,第一帧确定好了之后，然后通过金字塔Lucas-Kanade光流方法计算前期特征的光流（稀疏光流），具体使用OpenCV的方法calcOpticalFlowPyrLK
1.1.1 创建cv::TermCriteria类型变量termcrit（用于设置迭代算法终止条件）最大迭代次数和所期望的精度,两个加起来一个满足即可.
1.1.2 Opencv中LK法,具体构造函数(
img_prev 帧与帧之间的传递?
frame_cur->img_pyr_[0] 当前帧图像的金字塔
px_prev, //被跟踪特征点（上帧）坐标集合
px_cur, //当前帧特征点坐标集合
status,//输出状态向量status，用于表示每个特征是否被找到，找到就置1
error,//输出错误向量error，用于表示每个特征的错误（距离误差）。
cv::Size2i(klt_win_size, klt_win_size),//设置搜索窗口的大小为klt_win_size*klt_win_size。
3,//金字塔总层数为3.
termcrit,//终止条件,也就是上一步的那个终止条件
cv::OPTFLOW_USE_INITIAL_FLOW//利用px_cur中存储的坐标信息进行初步估计 )
1.1.3 清理f_cur(当前帧Keypoint方向向量集)和disparities(两帧之间的差距),然后将容器预留跟踪到的当前的keypoints数空间.
1.1.4 利用for循环剔除没有追踪到的特征点
1.1.5 将剩余特征点的像素坐标转换为世界坐标 通过frame类的函数c2f（调用cam2world函数）实现。然后再存入f_cur中。
1.1.6 将特征点移动的像素距离存入disparities中。 norm()范数
1.1.7 统计特征点特征类型 参考帧和当前帧keypoint坐标集数量 方向向量集 (这里仅仅是统计数量就ok吗??)
1.1.8 帧的金字塔复制给img_prev
1.1.9 把 当前帧的keypoints集合给px_prev
trackKlt函数完事
1.2 记录日志 "Init: KLT tracked "<< disparities_.size() <<" features" 根据1.1.3 和1.1.6 可以看出,disparities存储着特征点移动的距离 数量为检测到的特征数量
1.3 如果跟踪到的point数小于设定的最小值 失败 返回FAILURE 结束
1.4 调用getMedian函数得到disparities_中的中位数作为平均距离，幅值给变量disparity。
1.5 记录日志"Init: KLT "<<disparity<<"px average disparity."
1.6 如果中值小于给定配置参数，则表明这一帧不是关键帧，也就是刚开始的时候两帧不能太近
1.7 计算所有元素之和 得到距离的平均值 内积 (内积/数量-平均值的平方)的开平方 ??
1.8 计算单映性矩阵 输出 存储内点下标(inliers_) 存储3D点按估计的位姿投影得到的像素坐标(xyz_in_cur) 存储从和上帧到当前帧的位姿变化矩阵
1.9 记录日志 "Init: Homography RANSAC "<<inliers_.size()<<" inliers." 共找到多少内点
1.10 若内点数量小于设定阈值，则结束addSecondFrame函数，记录日志并返回FAILURE。
1.11 调节地图大小以使地图平均深度等于指定比例
把投影得到的像素坐标的z放在depth_vec容器 然后一个均值 取倒数得到scale
获得当前帧世界坐标系的位姿 然后对位移添加尺度(pos=-R-1次方乘t)
1.12 对每个内点 创建3Dpoint 和设置特征添加到这两帧中.
1.12.1 二维向量px_cur和px_ref用于存储内点特征点的像素坐标
1.12.2进行判断，若超出可视范围或者深度为负，则跳过，进行下一轮。
内点特征点坐标（相机坐标系）乘以scale后，得到其世界坐标系坐标，存入指针变量new_point
调用Frame类成员函数addFeature，将ftr_cur添加进frame_cur的fts（特征点列表）将同一个点对应的特征保存起来，这样点删除了，对应的特征都可以删除
调用Point类成员函数addFrameRef将ftr_cur添加进new_point的obs_（可以观测到此特征点的帧的指针的列表）
1.13 返回成功 到这里整个初始位置的确定就结束了，主要思想就是通过光流跟踪获得对应特征点对，通过对应特征点对估计单应矩阵，将单应矩阵进行分解或者相机外参数据，这个里面主要要注意的是检测到特征的点数，跟踪的特征点数，计算单应矩阵特征的内点数进行阈值限定，以及scale的估计.
2. 一个判断,若初始化失败或不是关键帧都会结束.
3.是否使用两帧之间的BA
4.将此帧设为关键帧.与processFirstFrame()中步骤3相同.
5.获取平均深度和最小深度.
6.将关键帧push_back到keyframes_中(List of keyframes in the map).
7.stage设置为STAGE_DEFAULT_FRAME.
8.初始化px_cur_和frame_ref_.
9.记录日志"Init: Selected second frame, triangulated initial map."
10.返回RESULT_IS_KEYFRAME.
processFrame()
1.设置初始位姿 将上一帧（last_frame_）的变换矩阵（T_f_w_）赋给当前帧的变换矩阵（T_f_w_）. ( 理下思路.这里正是贺一家大佬SVO论文分析里面 sparse model-based image alignment的step1.准备工作.)
2.记录:"sparse_img_align" 图像的稀疏对齐.
3.SparseImgAlign类型变量img_align，对构造函数进行初始化,图像金字塔最大层和最小层、迭代次数、采用高斯牛顿法、display_和verbose
4.调用 run函数进行对齐. (通过这个run函数 得到了优化后的粗略位姿(这里的位姿是Frame-to-Frame对齐得到的),也得到n_meas_/patch_area 也就是每个面片检测到的特征数.赋值给img_align_tracked)
(对于金字塔的处理这一步估计是从金字塔的顶层开始，把上一层的结果作为下一层估计的初始值，最后迭代到底层的。顶层的分辨率最小，所以这是一个由粗到精的过程（Coarse-to-Fine），使得在运动较大时也能有较好的结果。这一步没有找到在哪)
(完全可以使用优化库来解决问题)
4.1 reset()初始化.
4.2判断参考帧特征数是否为0 若是记录并结束.
4.3赋值
4.4 创建cv::Mat对象（行数为ref_frame_->fts_.size(),，列数为patch_area_（16），数值类型为CV_32F）并赋给ref_patch_cache_.(create n x 16 matrix).
4.5//调用resize函数对jacobian_cache_矩阵大小初始化,行不变,列设为ref_patch_cache_.rows*16. jacobian_cache_矩阵ColMajor按列存储 行为6 列动态
4.6 初始化向量visible_fts_(这个是什么向量)，使其长度为ref_patch_cache_.rows，值均为false。
4.7创建SE3型变量T_cur_from_ref（用于存储从上帧到当前帧的变换矩阵），初始化值为当前帧变换矩阵 乘以 上帧变换矩阵逆矩阵. Tji=Tjw*Tiw(-1)
4.8 for循环实现对变换矩阵的优化(level_在优化过程中当前帧的金字塔层数)
SparseAlign::optimize 这个优化得到了残差和雅克比 和H n_meas_/patch_area_ 同时优化了残差得到了粗略的位姿.....应该是作者自己写的,看高博知乎上的解答,是一种反向求导的方式.)
4.8.1 old_model 如果更新失败,保存旧的SE3
4.8.2 迭代估计
4.8.2.1 计算残差 赋值给new_chi2
4.8.2.1.2 如果linearize_system && display_ 就esimg_ = cv::Mat(cur_img.size(), CV_32F, cv::Scalar(0))
4.8.2.1.3 如果have_ref_patch_cache_ == false 就 提前计算参考帧的面片 不展开了.. 以上应该是文中注释的 对当前图像进行warp处理以对应参考图像.
4.8.2.1.4 在第一次迭代中计算权重
4.8.2.1.5 计算缓存的雅克比,对应每个特征
4.8.2.1.6 for循环对参考帧特征的遍历
4.8.2.1.6.1 检测特征在图像中是否可见
4.8.2.1.6.2 计算在当前图像中投影的像素位置
4.8.2.1.6.3 这个depth为两个点世界坐标系下相差的范数??
4.8.2.1.6.4 得到参考帧的特征在世界坐标系下的坐标,经过位姿转换得到当前帧的,经过world2cam得到相机坐标系下坐标,乘 尺度的像素位置
4.8.2.1.6.5 检测投影是否在图像中
4.8.2.1.6.6 对当前图像进行双边插值加权
4.8.2.1.6.7 不明白这啥float* ref_patch_cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
4.8.2.1.6.8 res = intensity_cur - (*ref_patch_cache_ptr);
4.8.2.1.6.9 遍历面片 计算残差chi2 += res*res*weight
4.8.2.1.6.10 然后这个n_meas_ ++
4.8.2.1.6.11 如果线性系统,计算雅克比,带权重的Hessian和残差图像
4.8.2.2 判断是否solve, solve是求解过程 若求解过程停止 返回 H和J
4.8.2.3 判断从上次优化后 误差是否增大,若增大,model=old_model
4.8.2.4 更新model. 调用了update函数具体的为 乘exp(se3李代数)
4.8.2.5 更新后的model即 new_model赋值给 model, 以前的model赋值给old_model(主要用来 4.8.2.3的判断)
4.8.2.6 更新残差
4.8.2.7 如果verbose_为真 输出 迭代的下标 残差 观测数量 norm_max(x_) x_是se3的李代数形式 即6*1矩阵
4.8.2.8 结束迭代估计
4.9 使用优化后的变换矩阵T_cur_from_ref乘以上一帧的变换矩阵 Tjw=Tji*Tiw (这里优化后的位姿)
4.10 返回n_meas_/patch_area_（值为16）给img_align_n_tracked 即frame_handler_mono中的函数
5. 记录日志 图像对齐结束时间
6. 记录 4.10得到的结果
7. SVO_DEBUG_STREAM("Img Align:\t Tracked = " << img_align_n_tracked);
//SVO_DEBUG_STREAM("hyj Direct Align, T_f_w: " << new_frame_->T_f_w_);
8. 记录重投影开始时间 而这里的重投影是帧与地图( Frame-to-Map)之间的重投影,注意与上面的帧与帧(Frame-to-Frame)之间的区别
9.reprojectMap 具体是找到与当前帧有相关视野最靠近的N个关键帧 ，这边设置N的最大值为10,靠近的N个关键帧的个数不超过10个。遍历这N个关键帧，对每个关键帧观察到的点投影到当前帧中，记录这每个关键帧与当前帧共同的观察点的个数
9.1 resetGrid() grid的顺序做了一次随机排序
9.2 得到跟当前帧有重叠视野的所有关键帧
9.3 根据靠近程度进行排序
9.4 对最近的N个关键帧进行迭代，找到有重叠视野的关键帧
9.4.1 将参考帧的img()clone赋值给debug_img_
9.4.2 将参考帧的指针 和 size_t(这表示下标?) push_back到overlap_kfs
9.4.3 for循环 将排好序的每个关键帧遍历每个特征点,根据特征点找到对应的map point,再投影到当前帧.
9.4.3.1 检测这个特征是否有分配的mappoint
9.4.3.2 确保只投影一次,不同帧上的特征会对应同一个3D点
9.4.3.3 统计相同观察点的数目
9.5 reproject candidates (投影那些还未插入关键帧的point_candidates_)
9.6 feature_align (特征块匹配，类似于光流,位置求精操作)
9.6.1 这样cur_frame的每个cell里就有了多个投影点, 每个cell只挑选1个质量最好的特征点。注意，这里作者为了速度考虑，不是对图像上的所有cell都挑选，随机挑选了maxFts这么多个cell。这就意味着并不是所有的投影地图点都会来用后面的pose,struct 优化。程序中grid_.cell_order[i]是用随机函数打乱了cell的排序.
在feature_align中的reprojectCell点进去,有一个findMatchDirect点进去,可以找到贺一家大佬在SVO代码笔记中所说的getCloseViewObs函数.
n_matches_ 多少个块匹配
n_trails_在reprojectCell中体现,重投影了多少特征点
10. 记录重投影结束时间
11. 进行判断，如果匹配到的特征数小于阈值，则打印没有匹配到足够的特征信息，同时设置当前帧变换矩阵为上帧变换矩阵，设置tracking_quality为TRACKING_INSUFFICIENT，并返回RESULT_FAILURE。
12. 记录pose_optimizer优化开始时间
13. 高斯牛顿的优化.(这个不点开了)(根据投影误差丢掉一些误差大的特征点，最后留下来进行位姿优化的这些特征点被变量sfba_n_edges_final记录下来。)
14. 判断 sfba_n_edges_final 小于20 失败
15. 结构优化 输入 frame 每次迭代中最大point数 最大迭代次数max_iter
15.1 对此帧的特征点迭代 如果特征点对应的point不为空 push_back到pts
15.2 设置最大point数为 min(max_n_pts, pts.size())
15.3 nth_element(标准模板函数 使第N大元素排在第N个位子,前面的比第N个元素小,后面的大)
15.4 调用了一个 point.cpp的一个优化函数optimize
15.5 统计优化次数?
16. 将当前帧插入core_kfs_（用于存储附近关键帧）
17. 跟踪质量设置为 sfba_n_edges_final(利用它来判断跟踪质量好不好setTrackingQuality(sfba_n_edges_final). 跟踪不好的判断依据是，用于位姿优化的点数sfba_n_edges_final小于一个阈值，或者比上一帧中用于优化的点减少了很多.)
17.1 如果被跟踪的特征数量小于设定的值 qualityMinFts 则跟踪bad
17.2 将feature_drop 设为 sfba_n_edges_final
17.3 drop/sfba_n_edges_final 值与 0.6比较 若大 则匹配bad
18. 判断tracking_quality_ ，若等于TRACKING_INSUFFICIENT，同时设置当前帧变换矩阵为上帧变换矩阵，并返回RESULT_FAILURE。
19. 获取场景最小和平均深度
20. 根据平均深度判断是否符合关键帧选择标准，若不合适或者tracking_quality_ 值为 TRACKING_BAD,就将当前帧添加入深度滤波器，然后返回RESULT_NO_KEYFRAME
21.当新的帧new frame和相邻KF的平移量超过场景深度平均值的12%时(比如四轴上升)，new frame就会被当做KF.
22. 记录日志 新的关键帧被选取
23. 遍历此帧的特征点 将map_.point_candidates_中与当前帧相关的特征点添加到当前帧。
24. 将当前关键帧添加到深度滤波器。
25. 如果关键帧的数量达到限制,移除最远一帧
26. 添加当前关键帧到map_
relocalizeFrame
1.调用map_.getClosestKeyframe函数查找最近的关键帧并赋给ref_keyframe.
2.判断ref_keyframe值，若为nullptr，则结束并返回RESULT_FAILURE。
3.调用SparseAlign类并命名为img_align进行图像对齐
4.调用run函数对齐,赋值n_meas_/patch_area_ (每个面片的特征数)给img_align_n_tracked
5.如果每个面片匹配特征数大于30，就将上帧变换矩阵赋给T_f_w_last，设置last_frame为ref_keyframe，然后调用processFram()函数，返回值保存到res，然后执行下一步
6. 判断res，若值不等于RESULT_FAILURE，就将stage_设置为STAGE_DEFAULT_FRAME，并打印 重定位成功 信息。否则，就将当前帧变换矩阵设置为T_f_w_last（即最近关键帧变换矩阵）。结束重定位函数，并返回res。
7. 如果匹配特征数小于等于30，就结束重定位函数，并返回RESULT_FAILURE。
参考: 高博在知乎上对SVO的解答.
贺一家大佬的SVO: semi-direct visual odometry 论文解析
及另一篇博客:SVO代码笔记.