精华内容
下载资源
问答
  • SLAN2 Mono ORBvoc.txt路径 TUM1.yaml路径) 这里的 /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/Monocular/TUM1.yaml 似乎应该是相机内参,在: /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/...

    1、ORB_SLAM2利用笔记本摄像头跑单目

    1、下载usb_cam源码并配置环境:

    cd catkin_ws/src
    git clone https://github.com/bosch-ros-pkg/usb_cam.git
    cd ..
    ctakin_make
    

    注意这里我的catkin_ws是之前编译过的,如果使用未编译的文件夹首次编译后需要添加source路径,在前面很多篇中都提到,不赘述。

    2、测试摄像头

    roslaunch usb_cam usb_cam-test.launch
    

    正常情况下是能够显示摄像头图像的,如果没有的话要思考前面的编译过程是否有问题。应该来说没什么问题。

    3、用ORB-SLAM2实时跑数据

    $ rosrun ORB_SLAM2 Mono /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/Monocular/TUM1.yaml    
           
    

    (即 rosrun ORB-SLAN2 Mono ORBvoc.txt路径 TUM1.yaml路径)
    在这里插入图片描述
    这里的

    /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/Monocular/TUM1.yaml
    

    似乎应该是相机内参,在:

    /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml  
    

    路径下也有一个对应的可运行的参数文件,所以这里如果执行:

    rosrun ORB_SLAM2 Mono /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml  
    

    同样也是可以运行的。
    在这里插入图片描述

    另外这里还有一个小问题:

    如果运行ORB_SLAM2之后图像显示不出来,可能是因为topic的问题,这时候可以查看发布的topic以及订阅的topic是否相同。gazebo插件中发布的topic可以使用:

    rostopic list
    

    命令查看,一般usb摄像头发布的图像格式应该是:usb_cam/image_raw
    而我们订阅的程序在:catkin/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src路径下的ros_mono.cc文件中,可以看到它的ros::subscriber一般初始化为:camera/image_raw。这里将其修改为上面的topic即可。

    2、ORB_SLAM2利用gazebo仿真环境跑单目仿真

    关于仿真环境的建立,具体可以参考之前写的:ROS学习总结十:Gazebo物理仿真环境搭建,这里就不赘述了。

    例如下面的是我随手建立的一个gazebo仿真环境(中间蓝色的是我的机器人):
    在这里插入图片描述
    然后使用三个终端分别打开下面的命令:

    $ roscore
    $ roslaunch mbot_gazebo view_mbot_with_camera_gazebo.launch
    $ rosrun ORB_SLAM2 Mono /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/Monocular/TUM1.yaml 
    

    注意这里中间的:

    $ roslaunch mbot_gazebo view_mbot_with_camera_gazebo.launch
    

    为我的gazebo启动程序,不一定相同,根据自己的路径设定。

    另外注意image的rostopic与ORB_SLAM2中订阅的topic问题。

    正常来说这时候就可以运行了,我们看一下效果:

    在这里插入图片描述
    嗯。。。效果不怎么样,发现在gazebo仿真中的特征点非常少,虽然我给了很多的特征了但是不知道为什么并没有被识别。

    于是我又改用了另一个参数运行:

    rosrun ORB_SLAM2 Mono /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml  
    

    得到新的效果:

    在这里插入图片描述
    虽然效果比第一个好,但是特征跟使用笔记本摄像头相比差的真不是一点半点。

    原本以为是gazebo仿真环境的问题,但是后来参考了一篇博客才发现应该还是参数的问题,在该博客中给出了另一种参数配置:

    https://www.freesion.com/article/6807208904/
    

    就是将原来TUM3.yaml中的参数:

    # Camera calibration and distortion parameters (OpenCV) 
    Camera.fx: 535.4
    Camera.fy: 539.2
    Camera.cx: 320.1
    Camera.cy: 247.6
    
    Camera.k1: 0.0
    Camera.k2: 0.0
    Camera.p1: 0.0
    Camera.p2: 0.0
    
    # Camera frames per second 
    Camera.fps: 30.0
    
    # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
    Camera.RGB: 1
    

    改成新的:

    # Camera calibration and distortion parameters (OpenCV) 
    Camera.fx: 277.19135641132203
    Camera.fy: 277.19135641132203
    Camera.cx: 160.5
    Camera.cy: 120.5
    
    Camera.k1: 0.0
    Camera.k2: 0.0
    Camera.p1: 0.0
    Camera.p2: 0.0
    
    Camera.width: 320
    Camera.height: 240
    
    # Camera frames per second 
    Camera.fps: 50.0
    
    # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
    Camera.RGB: 1
    

    为了保险起见最好重命名一个TUM4.yaml。

    重新运行以后特征提取上确实好了很多:
    在这里插入图片描述
    但是似乎特征点数量还是不太够,最终我绕着跑了一圈都没有初始化成功:
    在这里插入图片描述
    于是我使用了ROS-Academy-for-Beginners功能包,这个可以直接在GitHub上下载编译,编译过程与其他ROS功能包相同,中间可能会提示缺少几个依赖项,使用sudo apt-get install 安装即可

    编译后通过运行:

    $ roslaunch robot_sim_demo robot_spawn.launch
    

    打开gazebo环境,然后运行:

    $ rosrun ORB_SLAM2 Mono /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/zhangxingsheng/catkin_ws/src/ORB_SLAM2/Examples/Monocular/TUM4.yaml
    

    可以看到新运行的环境比之前的环境中多了很多的特征点。

    启动键盘节点运动小车初始化相机:

    $ roslaunch mbot_teleop mbot_teleop.launch
    

    在这里遇到了单目调试过程中最大的一个坑:
    每次我初始化结束后,下面map viewer上有位姿之后程序立马闪退:
    在这里插入图片描述
    就到这里然后报错:
    在这里插入图片描述
    吐槽一下:

    segmentation fault(core dumped)真的是难搞

    花了三天各种找原因,试了一万种方法,差点崩溃,最后在这里:

    https://blog.csdn.net/qq_43525260/article/details/104378643?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.channel_param&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.channel_param
    

    找到了解决方法。

    只是cmake.list文件中多出的一个-march=native导致了程序各种bomb。关于-march=native,百度了一个解释是:-march=[native]选项:gcc/g++编译器通过-march指定cpu架构,指定该选项之后编译器将不会生成兼容的指令集,而是该架构支持的特定指令集,可以取得一部分优化的效果。特殊地,-march=native选项让编译器获取当前机器的cpu架构,并生成该架构的最优指令,达到优化指令集的目的。不是很能理解。

    最终我通过将:

    ORB_SLAM2/Thirdparty/DBoW2/CMakeLists.txt
    ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt
    ORB_SLAM2/CMakeLists.txt
    ORB_SLAM2/Thirdparty/g2o/CMakeLists.txt
    

    中的-march=native全部删除后,按照之前的编译方式重新全部编译一次以后,终于可以正常运行了。

    注意不只是要运行:

    ./build_ros.sh
    

    需要把前面的g2o以及DBoW2等包都要重新编译一遍,具体的翻看上一篇博客的编译过程。不赘述。

    然后看一下效果:

    在这里插入图片描述
    总体来说其实还行,但是似乎对于自身的定位是有点差的:

    例如当我绕场景跑完一圈以后得到的图像:

    在这里插入图片描述
    这起点与终点差的不是一点半点。

    不过幸好似乎有个回环检测,总算把自己给圆回去了:
    在这里插入图片描述
    这张看起来还有点样子,不过单纯从自定位的角度来说单目的效果真的不太好。

    3、ORB_SLAM2利用gazebo仿真环境跑双目仿真

    双目仿真使用的是stereo插件,相比于单目要复杂一点,主要参考于:

    https://www.jianshu.com/p/811e24e8965a
    

    首先需要修改相机参数,双目相机参数如下:

    <!-- stereo camera -->
      <gazebo reference="camera_link">
          <sensor type="multicamera" name="stereo_camera">
            <update_rate>30.0</update_rate>
            <camera name="left">
              <horizontal_fov>1.3962634</horizontal_fov>
              <image>
                <width>800</width>
                <height>800</height>
                <format>R8G8B8</format>
              </image>
              <clip>
                <near>0.02</near>
                <far>300</far>
              </clip>
              <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.007</stddev>
              </noise>
            </camera>
            <camera name="right">
              <pose>0 -0.07 0 0 0 0</pose>
              <horizontal_fov>1.3962634</horizontal_fov>
              <image>
                <width>800</width>
                <height>800</height>
                <format>R8G8B8</format>
              </image>
              <clip>
                <near>0.02</near>
                <far>300</far>
              </clip>
              <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.007</stddev>
              </noise>
            </camera>
            <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
              <alwaysOn>true</alwaysOn>
              <updateRate>0.0</updateRate>
              <cameraName>camera</cameraName>
              <imageTopicName>image_raw</imageTopicName>
              <cameraInfoTopicName>camera_info</cameraInfoTopicName>
              <frameName>camera_link</frameName>
              <hackBaseline>0.07</hackBaseline>
              <distortionK1>0.0</distortionK1>
              <distortionK2>0.0</distortionK2>
              <distortionK3>0.0</distortionK3>
              <distortionT1>0.0</distortionT1>
              <distortionT2>0.0</distortionT2>
            </plugin>
          </sensor>
        </gazebo>
    

    建议新建一个模型,复制原来的参数过来以后替换掉相机的部分,这样在保留原来的模型的基础上修改更少。

    然后再给一个相机配置文件:

    %YAML:1.0
    
    #--------------------------------------------------------------------------------------------
    # Camera Parameters. Adjust them!
    #--------------------------------------------------------------------------------------------
    
    # Camera calibration and distortion parameters (OpenCV) 
    Camera.fx: 277.19135641132203
    Camera.fy: 277.19135641132203
    Camera.cx: 160.5
    Camera.cy: 120.5
    
    Camera.k1: 0.0
    Camera.k2: 0.0
    Camera.k3: 0.0
    Camera.p1: 0.0
    Camera.p2: 0.0
    
    Camera.width: 752
    Camera.height: 480
    
    # Camera frames per second 
    Camera.fps: 20.0
    
    # stereo baseline times fx
    Camera.bf: 47.90639384423901
    
    # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
    Camera.RGB: 1
    
    # Close/Far threshold. Baseline times.
    ThDepth: 35
    
    #--------------------------------------------------------------------------------------------
    # Stereo Rectification. Only if you need to pre-rectify the images.
    # Camera.fx, .fy, etc must be the same as in LEFT.P
    #--------------------------------------------------------------------------------------------
    LEFT.height: 480
    LEFT.width: 752
    LEFT.D: !!opencv-matrix
       rows: 1
       cols: 5
       dt: d
       data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]
    LEFT.K: !!opencv-matrix
       rows: 3
       cols: 3
       dt: d
       data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0]
    LEFT.R:  !!opencv-matrix
       rows: 3
       cols: 3
       dt: d
       data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
    LEFT.P:  !!opencv-matrix
       rows: 3
       cols: 4
       dt: d
       data: [435.2046959714599, 0, 367.4517211914062, 0,  0, 435.2046959714599, 252.2008514404297, 0,  0, 0, 1, 0]
    
    RIGHT.height: 480
    RIGHT.width: 752
    RIGHT.D: !!opencv-matrix
       rows: 1
       cols: 5
       dt: d
       data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0]
    RIGHT.K: !!opencv-matrix
       rows: 3
       cols: 3
       dt: d
       data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1]
    RIGHT.R:  !!opencv-matrix
       rows: 3
       cols: 3
       dt: d
       data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644]
    RIGHT.P:  !!opencv-matrix
       rows: 3
       cols: 4
       dt: d
       data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0]
    
    #--------------------------------------------------------------------------------------------
    # ORB Parameters
    #--------------------------------------------------------------------------------------------
    
    # ORB Extractor: Number of features per image
    ORBextractor.nFeatures: 1200
    
    # ORB Extractor: Scale factor between levels in the scale pyramid   
    ORBextractor.scaleFactor: 1.2
    
    # ORB Extractor: Number of levels in the scale pyramid  
    ORBextractor.nLevels: 8
    
    # ORB Extractor: Fast threshold
    # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
    # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
    # You can lower these values if your images have low contrast           
    ORBextractor.iniThFAST: 20
    ORBextractor.minThFAST: 7
    
    #--------------------------------------------------------------------------------------------
    # Viewer Parameters
    #--------------------------------------------------------------------------------------------
    Viewer.KeyFrameSize: 0.05
    Viewer.KeyFrameLineWidth: 1
    Viewer.GraphLineWidth: 0.9
    Viewer.PointSize:2
    Viewer.CameraSize: 0.08
    Viewer.CameraLineWidth: 3
    Viewer.ViewpointX: 0
    Viewer.ViewpointY: -0.7
    Viewer.ViewpointZ: -1.8
    Viewer.ViewpointF: 500
    

    编写launch文件:
    orbslam2_stereo.launch:

    <launch>
        <arg name="PATH_TO_VOCABULARY" value="$(find ORB_SLAM2)/vocabulary_files/ORBvoc.txt"/>
        <arg name="PATH_TO_SETTINGS_FILE" value="$(find ORB_SLAM2)/setting_files/Stereo_setting.yaml"/>
        
        <node name="Stereo" pkg="ORB_SLAM2" type="Stereo" args="$(arg PATH_TO_VOCABULARY) $(arg PATH_TO_SETTINGS_FILE) false">
        </node>
    
    </launch>
    

    在这里要说明一下ORB_SLAM2的节点启动,最后有一个false,是代表是否需要双目校正,在这里填ture的话,反而会出现严重的畸变,因此推测,仿真出来的相机镜头是不需要标定和校正的。

    启动命令:

    roslaunch ORB_SLAM2 orbslam2_stereo.launch
    

    这里的ORB_SLAM2是上面launch文件的一级路径,这里在我使用这个roslaunch的时候会出现参数文件找不到的问题,是因为它对应这里的ORB_SLAM2是指的catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2路径,而我们前面的参数文件是以catkin_ws/src/ORB_SLAM2作为路径的,简单点的方法就是将前面的参数以这个路径重新保存一份,只保存其中的vox.txt以及yaml文件即可。

    运行结果:
    在这里插入图片描述

    在这里插入图片描述
    可以看到在我快走完一圈的时候,整个位姿的偏差其实还是非常大的,跟单目的效果感觉差不多

    最后也是靠一个回环检测拯救了过来:

    在这里插入图片描述

    4、ORB_SLAM2利用gazebo仿真环境跑RGBD仿真

    同样的修改一下相机模型参数:

      <!-- camera -->
        <gazebo reference="camera_link">  
          <sensor type="depth" name="camera">
            <always_on>true</always_on>
            <update_rate>20.0</update_rate>
            <camera>
              <horizontal_fov>${60.0*3.14/180.0}</horizontal_fov>
              <image>
                  <format>R8G8B8</format>
                  <width>640</width>
                  <height>480</height>
              </image>
              <clip>
                  <near>0.05</near>
                  <far>8.0</far>
              </clip>
            </camera>
            <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
              <cameraName>camera</cameraName>
              <alwaysOn>true</alwaysOn>
              <updateRate>10</updateRate>
              <imageTopicName>rgb/image_raw</imageTopicName>
              <depthImageTopicName>depth/image_raw</depthImageTopicName>
              <pointCloudTopicName>depth/points</pointCloudTopicName>
              <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
              <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
              <frameName>camera_frame_optical</frameName>
              <baseline>0.1</baseline>
              <distortion_k1>0.0</distortion_k1>
              <distortion_k2>0.0</distortion_k2>
              <distortion_k3>0.0</distortion_k3>
              <distortion_t1>0.0</distortion_t1>
              <distortion_t2>0.0</distortion_t2>
              <pointCloudCutoff>0.4</pointCloudCutoff>
            </plugin>
          </sensor>
        </gazebo>
    

    这里我重新复制了ROS-Academy-for-Beginners功能包下的ros_sim_demo/xbot-ugazebo,重命名xbot_kinect,这个里面本身使用的是双目相机,然后将相机参数部分修改掉。注意修改robot.xacro文件中的模型名字,因为launch文件调用的是robot.xacro文件。

    launch文件可以不用写,因为在ROS-Academy-for-Beginners功能包下的orbslam2_demo下有一个对应的launch文件,打开看一下我们可以知道它是一个对应的RGBD相机的launch文件,可以直接使用这个launch文件。

    <launch>
        <arg name="PATH_TO_VOCABULARY" value="$(find ORB_SLAM2)/../../../Vocabulary/ORBvoc.txt"/>
        <arg name="PATH_TO_SETTINGS_FILE" value="$(find orbslam2_demo)/param/Zdzn.yaml "/>
        
        <node name="RGBD" pkg="ORB_SLAM2" type="RGBD" args="$(arg PATH_TO_VOCABULARY) $(arg PATH_TO_SETTINGS_FILE)"/>
       	 <remap from="/camera/depth_registered/image_raw" to="/camera/depth/image_raw"/>
    </launch>
    

    然后我们分别运行下列命令:

    $ roscore
    $ roslaunch robot_sim_demo robot_spawn.launch
    $ roslaunch orbslam2_demo ros_orbslam2.launch
    $ roslaunch mbot_teleop mbot_teleop.launch
    

    最终我们运行得到:

    这是快走完一圈时的路径:

    在这里插入图片描述
    这是走完一圈回环检测后的路径:

    在这里插入图片描述
    只能疯狂赞扬一下ORB_SLAM2的回环检测是真的强大,每次跑到天上去都能通过回环检测圆回来是真的可以。

    但是这个前端位置检测真的一言难尽,不知道是不是我相机参数的问题,有知道的小伙伴可以评论告知一下。

    展开全文
  • ORB-SLAM2 ORB特征点法SLAM 支持单目、双目rgbd相机 安装测试 本文github链接 orbslam2 + imu ORB-SLAM是一个基于特征点的实时单目SLAM系统,在大规模的、小规模的、室内室外的环境都可以运行。 该...

    ORB-SLAM2 ORB特征点法SLAM 支持单目、双目、rgbd相机

    安装测试

    本文github链接

    orbslam2 + imu

    ORB-SLAM是一个基于特征点的实时单目SLAM系统,在大规模的、小规模的、室内室外的环境都可以运行。
    该系统对剧烈运动也很鲁棒,支持宽基线的闭环检测和重定位,包括全自动初始化。
    该系统包含了所有SLAM系统共有的模块:
        跟踪(Tracking)、建图(Mapping)、重定位(Relocalization)、闭环检测(Loop closing)。
    由于ORB-SLAM系统是基于特征点的SLAM系统,故其能够实时计算出相机的轨线,并生成场景的稀疏三维重建结果。
    ORB-SLAM2在ORB-SLAM的基础上,还支持标定后的双目相机和RGB-D相机。
    

    系统框架

    贡献

    1. 相关论文:

    ORB-SLAM 单目Monocular特征点法

    ORB-SLAM2 单目双目rgbd

    词袋模型DBoW2 Place Recognizer

    原作者目录:

    Raul Mur-Artal

    Juan D. Tardos,

    J. M. M. Montiel

    Dorian Galvez-Lopez

    (DBoW2)

    2. 简介

    ORB-SLAM2 是一个实时的 SLAM  库,
    可用于 **单目Monocular**, **双目Stereo** and **RGB-D** 相机,
    用来计算 相机移动轨迹 camera trajectory 以及稀疏三维重建sparse 3D reconstruction 。
    
    在 **双目Stereo** 和 **RGB-D** 相机 上的实现可以得到真是的 场景尺寸稀疏三维 点云图
    可以实现 实时回环检测detect loops 、相机重定位relocalize the camera 。 
    提供了在  
    

    KITTI 数据集 上运行的 SLAM 系统实例,支持双目stereo 、单目monocular。

    TUM 数据集 上运行的实例,支持 RGB-D相机 、单目相机 monocular,

    EuRoC 数据集支持 双目相机 stereo 、单目相机 monocular.

    也提供了一个 ROS 节点 实时运行处理  单目相机 monocular, 双目相机stereo 以及 RGB-D 相机 数据流。
    提供一个GUI界面 可以来 切换 SLAM模式 和重定位模式
    

    **支持的模式:

    1. SLAM Mode  建图定位模式
        默认的模式,三个并行的线程: 跟踪Tracking, 局部建图 Local Mapping 以及 闭环检测Loop Closing. 
        定位跟踪相机localizes the camera, 建立新的地图builds new map , 检测到过得地方 close loops.
    
    2. Localization Mode 重定位模式
        适用与在工作地方已经有一个好的地图的情况下。执行 局部建图 Local Mapping 以及 闭环检测Loop Closing 两个线程.  
        使用重定位模式,定位相机
    

    3. 系统工作原理

    可以看到ORB-SLAM主要分为三个线程进行,
    也就是论文中的下图所示的,
    

    分别是Tracking、LocalMapping和LoopClosing。
    ORB-SLAM2的工程非常清晰漂亮,
    三个线程分别存放在对应的三个文件中,
    分别是:
    Tracking.cpp、
    LocalMapping.cpp 和
    LoopClosing.cpp 文件中,很容易找到。 
    

    A. 跟踪(Tracking)

      前端位姿跟踪线程采用恒速模型,并通过优化重投影误差优化位姿,
      这一部分主要工作是从图像中提取ORB特征,
      根据上一帧进行姿态估计,
      或者进行通过全局重定位初始化位姿,
      然后跟踪已经重建的局部地图,
      优化位姿,再根据一些规则确定新的关键帧。
    

    B. 建图(LocalMapping)

      局部地图线程通过MapPoints维护关键帧之间的共视关系,
      通过局部BA优化共视关键帧位姿和MapPoints,这一部分主要完成局部地图构建。
      包括对关键帧的插入,验证最近生成的地图点并进行筛选,然后生成新的地图点,
      使用局部捆集调整(Local BA),
      最后再对插入的关键帧进行筛选,去除多余的关键帧。
    

    C. 闭环检测(LoopClosing)

      闭环检测线程通过bag-of-words加速闭环匹配帧的筛选,
      并通过Sim3优化尺度,通过全局BA优化Essential Graph和MapPoints,
      这一部分主要分为两个过程:
      分别是: 闭环探测 和 闭环校正。
       闭环检测: 
            先使用WOB进行探测,然后通过Sim3算法计算相似变换。
       闭环校正:
            主要是闭环融合和Essential Graph的图优化。
    

    D. 重定位 Localization

    使用bag-of-words加速匹配帧的筛选,并使用EPnP算法完成重定位中的位姿估计。
    

    4. 代码分析

    ORB-SLAM2详解(二)代码逻辑

    4.1 应用程序框架

    单目相机app框架:

            1. 创建 单目ORB_SLAM2::System SLAM 对象
            2. 载入图片 或者 相机捕获图片 im = cv::imread();
            3. 记录时间戳 tframe ,并计时,
    	   std::chrono::steady_clock::now();    // c++11
    	   std::chrono::monotonic_clock::now();
            4. 把图像和时间戳 传给 SLAM系统, SLAM.TrackMonocular(im,tframe); 
            5. 计时结束,计算时间差,处理时间。
            6. 循环2-5步。
            7. 结束,关闭slam系统,关闭所有线程 SLAM.Shutdown();
            8. 保存相机轨迹, SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    

    双目相机app程序框架:

    	1. 读取相机配置文件(内参数 畸变矫正参数 双目对齐变换矩阵) =======================
    	   cv::FileStorage fsSettings(setting_filename, cv::FileStorage::READ);
    	   fsSettings["LEFT.K"] >> K_l;//内参数
    	   fsSettings["LEFT.D"] >> D_l;// 畸变矫正
    	   fsSettings["LEFT.P"] >> P_l;// P_l,P_r --左右相机在校准后坐标系中的投影矩阵 3×4
    	   fsSettings["LEFT.R"] >> R_l;// R_l,R_r --左右相机校准变换(旋转)矩阵  3×3
    	2. 计算双目矫正映射矩阵========================================================
    	   cv::Mat M1l,M2l,M1r,M2r;
               cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
               cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);
    	3. 创建双目系统=============================================================== 
    	   ORB_SLAM2::System SLAM(vocabulary_filepath, setting_filename, ORB_SLAM2::System::STEREO, true);
    	4. 从双目设备捕获图像,设置分辨率捕获图像========================================
    	   cv::VideoCapture CapAll(deviceid); //打开相机设备 
    	   //设置分辨率   1280*480  分成两张 640*480  × 2 左右相机
               CapAll.set(CV_CAP_PROP_FRAME_WIDTH,1280);  
               CapAll.set(CV_CAP_PROP_FRAME_HEIGHT, 480); 
            5. 获取左右相机图像===========================================================
    	   CapAll.read(src_img);
    	   imLeft  = src_img(cv::Range(0, 480), cv::Range(0, 640));   
               imRight = src_img(cv::Range(0, 480), cv::Range(640, 1280));   
    	6. 使用2步获取的双目矫正映射矩阵 矫正 左右相机图像==============================
    	   cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
               cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);
    	7. 记录时间戳 time ,并计时===================================================
    		#ifdef COMPILEDWITHC11
    		   std::chrono::steady_clock::time_point    t1 = std::chrono::steady_clock::now();
    		#else
    		   std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
    		#endif
            8. 把左右图像和时间戳 传给 SLAM系统===========================================
    	   SLAM.TrackStereo(imLeftRect, imRightRect, time);
    	9. 计时结束,计算时间差,处理时间============================================= 
    	10.循环执行 5-9步              =============================================
    	11.结束,关闭slam系统,关闭所有线程===========================================   
    	12.保存相机轨迹                   ========================================== 
    

    ORB_SLAM2::System SLAM 对象框架:

            在主函数中,我们创建了一个ORB_SLAM2::System的对象SLAM,这个时候就会进入到SLAM系统的主接口System.cc。
            这个代码是所有调用SLAM系统的主入口,
            在这里,我们将看到前面博客所说的ORB_SLAM的三大模块:
            Tracking、LocalMapping 和 LoopClosing。
    
        System类的初始化函数:
            1. 创建字典 mpVocabulary = new ORBVocabulary();并从文件中载入字典 
               mpVocabulary = new ORBVocabulary();           // 创建关键帧字典数据库
               // 读取 txt格式或者bin格式的 orb特征字典, 
               mpVocabulary->loadFromTextFile(strVocFile);   // txt格式
               mpVocabulary->loadFromBinaryFile(strVocFile); // bin格式
               
            2. 使用特征字典mpVocabulary 创建关键帧数据库 
               mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
               
            3. 创建地图对象 mpMap 
               mpMap = new Map();
               
            4. 创建地图显示(mpMapDrawer) 帧显示(mpFrameDrawer) 两个显示窗口   
               mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);//地图显示
               mpFrameDrawer = new FrameDrawer(mpMap);//关键帧显示
               
            5. 初始化 跟踪线程(mpTracker) 对象 未启动
    	       mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
    				      mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor); 
                          
            6. 初始化 局部地图构建线程(mptLocalMapping) 并启动线程
    	       mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    	       mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
              
            7. 初始化 闭环检测线程(mptLoopClosing) 并启动线程
    	       mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    	       mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
              
            8. 初始化 跟踪线程可视化 并启动
               mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
    		   mptViewer = new thread(&Viewer::Run, mpViewer);
    		   mpTracker->SetViewer(mpViewer);
               
            9. 线程之间传递指针 Set pointers between threads
    	       mpTracker->SetLocalMapper(mpLocalMapper);   // 跟踪线程 关联 局部建图和闭环检测线程
    	       mpTracker->SetLoopClosing(mpLoopCloser);
    	       mpLocalMapper->SetTracker(mpTracker);       // 局部建图线程 关联 跟踪和闭环检测线程
    	       mpLocalMapper->SetLoopCloser(mpLoopCloser);
    	       mpLoopCloser->SetTracker(mpTracker);        // 闭环检测线程 关联 跟踪和局部建图线程
    	       mpLoopCloser->SetLocalMapper(mpLocalMapper);
    
        如下图所示: 
    

    单目跟踪SLAM.TrackMonocular()框架

    	1. 模式变换的检测  跟踪+定位  or  跟踪+定位+建图
    	2. 检查跟踪tracking线程重启
    	3. 单目跟踪
    	   mpTracker->GrabImageMonocular(im,timestamp);// Tracking.cc中
    	   // 图像转换成灰度图,创建帧Frame对象(orb特征提取等)
    	   // 使用Track()进行跟踪: 
    	                 0. 单目初始化(最开始执行) MonocularInitialization();// 单目初始化
    	                 a. 两帧跟踪得到初始化位姿(跟踪上一帧/跟踪参考帧/重定位)
    	                 b. 跟踪局部地图,多帧局部地图G2O优化位姿, 新建关键帧
    

    双目跟踪System::TrackStereo()框架

    	1. 模式变换的检测  跟踪+定位  or  跟踪+定位+建图
    	2. 检查跟踪tracking线程重启
    	3. 双目跟踪
    	   mpTracker->GrabImageStereo(imLeft,imRight,timestamp); // Tracking.cc中
    	   // 图像转换成灰度图,创建帧Frame对象(orb特征提取器,分块特征匹配,视差计算深度)
    	   // 使用Track()进行跟踪:
    	                 0. 双目初始化(最开始执行) StereoInitialization();// 双目 / 深度初始化
    	                 a. 两帧跟踪得到初始化位姿(跟踪上一帧/跟踪参考帧/重定位)
    		         b. 跟踪局部地图,多帧局部地图G2O优化位姿, 新建关键帧
    

    深度相机跟踪System::TrackRGBD()框架

    	1. 模式变换的检测  跟踪+定位  or  跟踪+定位+建图
    	2. 检查跟踪tracking线程重启
    	3. 双目跟踪
    	   mpTracker->GrabImageRGBD(im,depthmap,timestamp); // Tracking.cc中
    	   // 图像转换成灰度图,创建帧Frame对象(orb特征提取器,深度图初始化特征点深度)
    	   // 使用Track()进行跟踪: 
    	                 0. RGBD初始化(最开始执行) StereoInitialization();// 双目 / 深度初始化
    	                 a. 两帧跟踪得到初始化位姿(跟踪上一帧/跟踪参考帧/重定位)
    		         b. 跟踪局部地图,多帧局部地图G2O优化位姿, 新建关键帧
    

    4.2 单目/双目/RGBD初始化, 单应变换/本质矩阵恢复3d点,创建初始地图

    参考

    1. 单目初始化 Tracking::MonocularInitialization()

    系统的第一步是初始化,ORB_SLAM使用的是一种自动初始化方法。
    这里同时计算两个模型:
    1. 用于 平面场景的单应性矩阵H( 4对 3d-2d点对,线性方程组,奇异值分解) 
    2. 用于 非平面场景的基础矩阵F(8对 3d-2d点对,线性方程组,奇异值分解),
    

    推到 参考 单目slam基础

    然后通过一个评分规则来选择合适的模型,恢复相机的旋转矩阵R和平移向量t。
    函数调用关系:
    Tracking::GrabImageMonocular() 创建帧对象(第一帧提取orb特征点数量较多,为后面帧的两倍) -> 
    Tracking::Track() ->  初始化 MonocularInitialization();// 单目初始化
    	  |  1. 第一帧关键点个数超过 100个,进行初始化  mpInitializer = new Initializer(mCurrentFrame,1.0,200);
    	  |  2. 第二帧关键点个数 小于100个,删除初始化器,跳到第一步重新初始化。
    	  |  3. 第二帧关键点个数 也大于100个(只有连续的两帧特征点 均>100 个才能够成功构建初始化器)
    	  |     构建 两两帧 特征匹配器    ORBmatcher::ORBmatcher matcher(0.9,true)
    	  
    	  |     金字塔分层块匹配搜索匹配点对  100为搜索窗口大小尺寸尺度 
    	  |     int nmatches=matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
    	  
    	  |  4. 如果两帧匹配点对过少(nmatches<100),跳到第一步,重新初始化。
    	  |  5. 匹配点数量足够多(nmatches >= 100),进行单目初始化:
    	  |     第一帧位置设置为:单位阵 Tcw = cv::Mat::eye(4,4,CV_32F);
    	  |     使用 单应性矩阵H 和 基础矩阵F 同时计算两个模型,通过一个评分规则来选择合适的模型,
    	  |     恢复第二帧相机的旋转矩阵Rcw 和 平移向量 tcw,同时 三角变换得到 部分三维点 mvIniP3D
    	  |     mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
    	  |  6. 设置初始参考帧的世界坐标位姿态:
    	  |     mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
    	  |  7. 设置第二帧(当前帧)的位姿
    	  |     cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
    	  |     Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
    	  |     tcw.copyTo(Tcw.rowRange(0,3).col(3)); 
    	  |     mCurrentFrame.SetPose(Tcw);
    	  |  8. 创建初始地图 使用 最小化重投影误差BA 进行 地图优化 优化位姿 和地图点
    	  |     Tracking::CreateInitialMapMonocular()
    	  |
    	  |-> 后面帧的跟踪 -> 两两帧的跟踪得到初始位姿
    		  |  1. 有运动速度,使用恒速运动模式跟踪上一帧   Tracking::TrackWithMotionModel()
    		  |  2. 运动量小或者 1.跟踪失败,跟踪参考帧模式  Tracking::TrackReferenceKeyFrame()
    		  |  3. 1 和 2都跟踪失败的话,使用重定位跟踪,跟踪可视参考帧群 Tracking::Relocalization()
    		  | 
    	         之后 -> 跟踪局部地图(一级二级相连关键帧), 使用图优化对位姿进行精细化调整 Tracking::TrackLocalMap()
    		     |
    		     -> 判断是否需要新建关键帧   Tracking::NeedNewKeyFrame();  Tracking::CreateNewKeyFrame();
    		         |
    			 |-> 跟踪失败后的处理
    

    初始化时,orb匹配点的搜索 ORBmatcher::SearchForInitialization()

    金字塔分层分块orb特征匹配,然后通过三个限制,滤除不好的匹配点对:
    	1. 最小值阈值限制;
    	2. 最近匹配距离一定比例小于次近匹配距离;
    	3. 特征方向误差一致性判断(方向差直方图统计,保留3个最高的方向差一致性最多的点,一致性较大)
    	   ORBmatcher::ComputeThreeMaxima(); // 统计数组中最大 的几个数算法,参考性较大
    	   ORBmatcher.cc 1912行 优秀的算法值得参考
    步骤:
    	步骤1:为帧1的每一个关键点在帧2中寻找匹配点(同一金字塔层级,对应位置方块内的点,多个匹配点)
    	       Frame::GetFeaturesInArea()
    	步骤2:计算 1对多 匹配点对描述子之间的距离(二进制变量差异)
    	       ORBmatcher::DescriptorDistance(d1,d2); // 只计算了前八个 二进制位 的差异
    	       ORBmatcher.cc 1968行 优秀的算法值得参考
    	步骤3:保留最小和次小距离对应的匹配点
    	       ORBmatcher.cc 570行  优秀的算法值得参考
    	步骤4:确保最小距离小于阈值 50
    	步骤5:确保最佳匹配比次佳匹配明显要好,那么最佳匹配才真正靠谱,并统计方向差值直方图
    	步骤6:特征方向误差一致性判断(方向差直方图统计,保留3个最高的方向差一致性最多的点,一致性较大)
    	步骤7:更新匹配信息,用最新的匹配更新之前记录的匹配
    

    单目位姿恢复分析 Initializer::Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))

    步骤1:根据 matcher.SearchForInitialization 得到的初始匹配点对,筛选后得到好的特征匹配点对
    步骤2:在所有匹配特征点对中随机选择8对特征匹配点对为一组,共选择 mMaxIterations 组
    步骤3:调用多线程分别用于计算fundamental matrix(基础矩阵F) 和 homography(单应性矩阵)
       Initializer::FindHomography();   得分为SH
       Initializer::FindFundamental();  得分为SF
    步骤4:计算评价得分 RH,用来选取某个模型
           float RH = SH / (SH + SF);// 计算 选着标志
    步骤5:根据评价得分,从单应矩阵H 或 基础矩阵F中恢复R,t
       if(RH>0.40)// 更偏向于 平面  使用  单应矩阵恢复
         return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
       else //if(pF_HF>0.6) // 偏向于非平面  使用 基础矩阵 恢复
         return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    

    0. 2D-2D配点对求变换矩阵前先进行标准化处理去 均值后再除以绝对矩 Initializer::Normalize()

    步骤1:计算两坐标的均值
           mean_x  =  sum(ui) / N ;
           mean_y  =  sum(vi) / N;
    步骤2:计算绝对局倒数
          绝对矩:
    	 mean_x_dev = sum(abs(ui - mean_x))/ N ;
    	 mean_y_dev = sum(abs(vi - mean_y))/ N ; 
          绝对矩倒数:
    	 sX = 1/mean_x_dev 
    	 sY = 1/mean_y_dev
    步骤3:计算标准化后的点坐标
    	ui' = (ui - mean_x) * sX
    	vi' = (vi - mean_y) * sY 
    步骤4:计算并返回标准化矩阵 
    	ui' = (ui - mean_x) * sX =  ui * sX + vi * 0  + (-mean_x * sX) * 1
    	vi' = (vi - mean_y) * sY =  ui * 0  + vi * sY + (-mean_y * sY) * 1   
    	1   =                       ui * 0  + vi * 0  +      1         * 1
    
    	可以得到:
    		ui'     sX  0   (-mean_x * sX)      ui
    		vi' =   0   sY  (-mean_y * sY)   *  vi
    		1       0   0        1              1
    	标准化后的的坐标 = 标准化矩阵T * 原坐标
    	所以标准化矩阵:
    		T =  sX  0   (-mean_x * sX) 
    		     0   sY  (-mean_y * sY)
    		     0   0        1 
    	而由标准化坐标 还原 回 原坐标(左乘T 逆):
    		原坐标 = 标准化矩阵T 逆矩阵 * 标准化后的的坐标
    
    	再者用于还原 单应矩阵 下面需要用到:
    	  p1'  ------> Hn -------> p2'   , p2'   = Hn*p1'
    	  T1*p1 -----> Hn -------> T2*p2 , T2*p2 = Hn*(T1*p1)
    	  左乘 T2逆 ,得到   p2 = T2逆 * Hn*(T1*p1)= H21i*p1
    	  H21i = T2逆 * Hn * T1
    

    a. fundamental matrix(基础矩阵F) 随机采样 找到最好的 基础矩阵 Initializer::FindFundamental()

    思想:
    计算基础矩阵F,随机采样序列8点法,采用归一化的直接线性变换(normalized DLT)求解,
    极线几何约束(current frame 1 变换到 reference frame 2),
    在最大迭代次数内调用 ComputeF21计算F,
    使用 CheckFundamental 计算此 基础矩阵的得分,
    在最大迭代次数内保留最高得分的基础矩阵F。
    
    步骤:
    步骤1:将两帧上对应的2d-2d匹配点对进行归一化
    	Initializer::Normalize(mvKeys1,vPn1, T1);//  mvKeys1原坐标 vPn1归一化坐标,T1标准化矩阵
    	Initializer::Normalize(mvKeys2,vPn2, T2);// 
    	cv::Mat T2inv = T2.inv();// 标准化矩阵 逆矩阵
    步骤2:在最大迭代次数mMaxIterations内,从标准化后的点中随机选取8对点对
    	int idx = mvSets[it][j];//随机数集合 总匹配点数范围内
    	vPn1i[j] = vPn1[mvMatches12[idx].first];
    	vPn2i[j] = vPn2[mvMatches12[idx].second]; 
    步骤3:通过标准化逆矩阵 和 标准化点对的基础矩阵 计算原点对的 基础矩阵   
    	cv::Mat Fn = ComputeF21(vPn1i,vPn2i);// 计算 标准化后的点对 对应的 基础矩阵 Fn
    	cv::Mat T2t = T2.t(); // 标准化矩阵的 转置矩阵
    	 推到:
    	 x2 = R*x1 + t
    	 t 叉乘 x2 = t 叉乘 R*x1
    	 x2转置 * t 叉乘 x2 = x2转置 * t 叉乘 R*x1 = 0
    	 得到: 
    	 x2转置 * t 叉乘 R  * x1 = x2转置 * R21  * x1 =  0
    	 (K逆*p2)转置 * t 叉乘 R  * (K逆*p1) = 
    	 p2转置 * K转置逆 * t 叉乘 R  * K逆 * p1 = p2转置 * F21 *  p1
    
    	 上面求到的是 归一化后的点对的变换矩阵:
    	 p2'转置 * Fn * p1' = 0
    	 (T2*p2)转置 * Fn * (T1 * p1) = 0
    	 p2转置 * T2转置 * Fn * T1 * p1 = 0
    	 所以得到:
    	   未归一化点对对应的变换矩阵F21为:
    	   F21 =  T2转置 * Fn * T1 = T2t * Fn * T1
    
    步骤4:通过计算重投影误差来计算单应矩阵的好坏,得分 
    	currentScore =Initializer::CheckFundamental(); 
    步骤5:保留迭代中,得分最高的单应矩阵和对应的得分
    	if(currentScore > score)//此次迭代 计算的单应H的得分较高
    	{
    	    F21 = F21i.clone();// 保留最优的 基础矩阵 F
    	    vbMatchesInliers = vbCurrentInliers;//对应的匹配点对   标记内点
    	    score = currentScore;// 最高的得分
    	}
    

    Initializer::ComputeF21(vPn1i,vPn2i) 8对点求解 基础矩阵F

     推到:
     x2 = R*x1 + t
     t 叉乘 x2 = t 叉乘 R*x1
     x2转置 * t 叉乘 x2 = x2转置 * t 叉乘 R*x1 = 0
     得到: 
     x2转置 * t 叉乘 R  * x1 = x2转置 * R21  * x1 =  0
     (K逆*p2)转置 * t 叉乘 R  * (K逆*p1) = 
     p2转置 * K转置逆 * t 叉乘 R  * K逆 * p1 = p2转置 * F21 *  p1
    
     一对2d-2d点对 p1-p2得到:
    	 p2转置 * F21 *  p1
    写成矩阵形式:
                 |f1   f2   f3|     |u1|
    |u2 v2 1| *  |f4   f5   f6|  *  |v1|    = 0 , 应该=0 不等于零的就是误差
                 |f7   f8   f9|     |1|
    前两项展开得到:
    	a1 = f1*u2 + f4*v2 + f7;
    	b1 = f2*u2 + f5*v2 + f8;
    	c1 = f3*u2 + f6*v2 + f9;
    得到:
    		     |u1|
    	|a1 b1 c1| * |v1| = 0
    		     |1|
    得到:
           a1*u1+ b1*v1 + c1= 0
    一个点对 得到一个约束方程:
      f1*u1*u2 + f2*v1*u2  + f3*u2 + f4*u1*v2  + f5*v1*v2 + f6*v2 +  f7*u1 + f8*v1 + f9 = 0
    写成矩阵形式:
     |u1*u2 v1*u2 u2 u1*v2 v1*v2 v2 u1 v1 1| * |f1 f2 f3 f4 f5 f6 f7 f8 f9|转置 = 0
    采样8个点对 可以到八个约束, f 9个参数,8个自由度,另一个为尺度因子(单目尺度不确定的来源)
    线性方程组 求解 A * f = 0  
    对矩阵A进行奇异值分解得到f ,
    
    对A进行SVD奇异值分解 [U,S,V]=svd(A),其中U和V代表二个相互正交矩阵,而S代表一对角矩阵
    cv::SVDecomp(A,S,U,VT,SVD::FULL_UV);  //后面的FULL_UV表示把U和VT补充称单位正交方阵
    Fpre  = VT.row(8).reshape(0, 3);      // V的最后一列
    F矩阵的其秩为2,需要再对Fpre进行奇异值分解, 后取对角矩阵U, 秩为2,后再合成F。
    cv::SVDecomp(Fpre,S,U,VT,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    S.at<float>(2)=0;                     //  基础矩阵的秩为2,重要的约束条件
    F21 = U * cv::Mat::diag(S)* VT        // 再合成F
    
    

    Initializer::CheckFundamental() 计算基本矩阵得分 得分为SF

    思想:
    	1. 根据基本矩阵,可以求得两组对点相互变换得到的误差平方和,
    	2. 基于卡方检验计算出的阈值(假设测量有一个像素的偏差),
    	3. 误差大的记录为外点,误差小的记录为内点,
    	4. 使用阈值减去内点的误差,得到该匹配点对的得分(误差小的,得分高),
    	5. 记录相互变换中所有内点的得分之和作为该单元矩阵的得分,并更新匹配点对的内点外点标记。
    	
    步骤1: 误差阈值 th = 3.841; 得分最大值 thScore = 5.991; 误差方差倒数 invSigmaSquare
    步骤2: 遍历每一对2d-2d点对计算误差:
    
            如: p2 ->  F12  -> p1 
    		     |f1   f2   f3|     |u1|
    	|u2 v2 1| *  |f4   f5   f6|  *  |v1|    = 0 , 应该=0 不等于零的就是误差
    		     |f7   f8   f9|     |1|
    前两项展开得到:
    	a1 = f1*u2 + f4*v2 + f7;
    	b1 = f2*u2 + f5*v2 + f8;
    	c1 = f3*u2 + f6*v2 + f9;
    得到:
    		     |u1|
    	|a1 b1 c1| * |v1| = 0 ,其实就是点 (u1, v1) 在线段 (a1, b1, c1) 上的形式
    		     |1|
    极线l1:a1*x + b1*y + c1 = 0,这里把p2投影到帧1平面上对应的极线形式,p1应该在上面。
    点p1,(u1,v1) 到 l1的距离为:    
            d = |a1*u + b1*v + c| / sqrt(a1^2 + b1^2) 
    距离平方:
            chiSquare1 = d^2 = (a1*u + b1*v + c)^2 / (a1^2 + b1^2)
    
    根据方差归一化误差:
    	const float chiSquare1 = squareDist1*invSigmaSquare;
    	
    使用阈值更新内外点标记 并记录内点得分:
    	if(chiSquare1 > th)
    	    bIn = false;                  // 距离大于阈值  该点 变换的效果差,记录为外点
    	else
    	    score += thScore - chiSquare1;// 得分上限 - 距离差值 得到 得分,差值越小,得分越高      得分为SF
    
    同时记录 p1 ->  F21  -> p2 的误差,也如上述步骤。
    更新内外点记录数组:
    	if(bIn)
    		vbMatchesInliers[i]=true;// 是内点  误差较小
    	else
    		vbMatchesInliers[i]=false;// 是野点 误差较大
    

    b. homography(单应性矩阵) 随机采样 找到最好的单元矩阵 Initializer::FindHomography()

    思想:
    计算单应矩阵,随机采样序列4点法,采用归一化的直接线性变换(normalized DLT)求解,
    假设场景为平面情况下通过前两帧求取Homography矩阵(current frame 2 到 reference frame 1),
    在最大迭代次数内调用 ComputeH21计算H,
    使用 CheckHomography 计算此单应变换得分,
    在最大迭代次数内保留最高得分的单应矩阵H。
    
    步骤:
    步骤1:将两帧上对应的2d-2d匹配点对进行归一化
    	Initializer::Normalize(mvKeys1,vPn1, T1);//  mvKeys1原坐标 vPn1归一化坐标,T1标准化矩阵
    	Initializer::Normalize(mvKeys2,vPn2, T2);// 
    	cv::Mat T2inv = T2.inv();                // 标准化矩阵 逆矩阵
    步骤2:在最大迭代次数mMaxIterations内,从标准化后的点中随机选取8对点对
    	int idx = mvSets[it][j];                 //随机数集合 总匹配点数范围内
    	vPn1i[j] = vPn1[mvMatches12[idx].first];
    	vPn2i[j] = vPn2[mvMatches12[idx].second]; 
    步骤3:通过标准化逆矩阵和标准化点对的单应变换计算原点对的单应变换矩阵 Initializer::ComputeH21()
    	cv::Mat Hn = ComputeH21(vPn1i,vPn2i);// 计算 标准化后的点对 对应的 单应矩阵Hn
    	// H21i = T2逆 * Hn * T1  见上面  0步骤的推导
    	H21i = T2inv * Hn * T1;// 原始点    p1 -----------> p2 的单应
    	H12i = H21i.inv();     // 原始点    p2 -----------> p1 的单应
    步骤4:通过计算重投影误差来计算单应矩阵的好坏,得分 
    	currentScore =Initializer::CheckHomography(); 
    步骤5:保留迭代中,得分最高的单应矩阵和对应的得分
    	if(currentScore > score)//此次迭代 计算的单应H的得分较高
    	{
    	    H21 = H21i.clone();//保留较高得分的单应
    	    vbMatchesInliers = vbCurrentInliers;//对应的匹配点对   
    	    score = currentScore;// 最高的得分
    	}
    

    Initializer::ComputeH21() 4对点直接线性变换求解H矩阵

    一点对:
    	p2   =  H21 * p1
    写成矩阵形式:
    	u2         h1  h2  h3       u1
    	v2  =      h4  h5  h6    *  v1
    	1          h7  h8  h9       1  
    
    可以使用叉乘 得到0    p2叉乘p2 = H21 *p1 = 0 
    	| 0 -1  v2|    |h1 h2 h3|      |u1|    |0|
    	| 1  0 -u2| *  |h4 h5 h6| *    |v1| =  |0|
    	|-v2 u2  0|    |h7 h8 h9|      |1 |    |0|
    
    也可以展开得到(使用第三项进行归一化):
    	u2 = (h1*u1 + h2*v1 + h3) /( h7*u1 + h8*v1 + h9)
    	v2 = (h4*u1 + h5*v1 + h6) /( h7*u1 + h8*v1 + h9)
    写成矩阵形式:
    	-((h4*u1 + h5*v1 + h6) - ( h7*u1*v2 + h8*v1*v2 + h9*v2))=0  右乘分子,再移动得0
    	h1*u1    + h2*v1 + h3  - ( h7*u1*u2 + h8*v1*u2 + h9*u2) =0
    
    	|0    0   0  -u1  -v1  -1   u1*v2   v1*v2    v2|
    	|u1 v1    1  0    0    0   -u1*u2  - v1*u2  -u2| *|h1 h2 h3 h4 h5 h6 h7 h8 h9|转置  = 0
    一对点提供两个约束:H 9个元素,8个自由度,包含一个比例因子(尺度来源),需要四对点
    四对点提供8个约束方程,可以写成矩阵形式:
     A * h = 0
    对A进行SVD奇异值分解 [U,S,V]=svd(A),其中U和V代表二个相互正交矩阵,而S代表一对角矩阵
    cv::SVDecomp(A,S,U,VT,SVD::FULL_UV);  //后面的FULL_UV表示把U和VT补充称单位正交方阵	
    H = VT.row(8).reshape(0, 3);// v的最后一列
    
    SVD奇异值分解 解齐次方程组(Ax = 0)原理:
    	把问题转化为最小化|| Ax ||2的非线性优化问题,
    	我们已经知道了x = 0是该方程组的一个特解,
    	为了避免x = 0这种情况(因为在实际的应用中x = 0往往不是我们想要的),
    	我们增加一个约束,比如|| x ||2 = 1,
    	这样,问题就变为:
    	min(|| Ax ||2) , || x ||2 = 1 或 min(|| Ax ||) , || x || = 1
    	对矩阵A进行分解 A = UDV'
    	 || Ax || = || UDV' x || = || DV' x||
    	 ( 对于一个正交矩阵U,满足这样一条性质: || UD || = || D || , 
    	   正交矩阵,正交变换,仅仅对向量,只产生旋转,无尺度缩放,和变形,即模长不变)
    
    	令y = V'x, 因此,问题变为min(|| Dy ||), 
    	因为|| x || = 1, V'为正交矩阵,则|| y || = 1。
    	由于D是一个对角矩阵,对角元的元素按递减的顺序排列,因此最优解在y = (0, 0,..., 1)'
    	又因为x = Vy, 所以最优解x,就是V的最小奇异值对应的列向量,
    	比如,最小奇异值在第8行8列,那么x = V的第8个列向量。
    

    计算单应变换得分 Initializer::CheckHomography() 得分为SH

    思想:
    	1. 根据单应变换,可以求得两组对点相互变换得到的误差平方和,
    	2. 基于卡方检验计算出的阈值(假设测量有一个像素的偏差),
    	3. 误差大的记录为外点,误差小的记录为内点,
    	4. 使用阈值减去内点的误差,得到该匹配点对的得分(误差小的,得分高),
    	5. 记录相互变换中所有内点的得分之和作为该单元矩阵的得分,并更新匹配点对的内点外点标记。
    
    步骤1:获取单应变换误差阈值th,以及误差归一化的方差倒数    
    	const float th = 5.991;                        // 单应变换误差 阈值
    	const float invSigmaSquare = 1.0/(sigma*sigma);//方差 倒数,用于将误差归一化
    
    步骤2:遍历每个点对,计算单应矩阵 变换 时产生 的 对称的转换误差
      p1点 变成 p2点  p2 = H21*p1---------------------------------------
    	|u2'|      |h11  h12  h13|     |u1|    |h11*u1 + h12*v1 + h13|
    	|v2'|  =   |h21  h22  h23|  *  |v1| =  |h21*u1 + h22*v1 + h23|
    	|1|        |h31  h32  h33|     |1|     |h31*u1 + h32*v1 + h33|
    	 使用第三行进行归一化: u2' = (h11*u1 + h12*v1 + h13)/(h31*u1 + h32*v1 + h33);
    			      v2' = (h21*u1 + h22*v1 + h23)/(h31*u1 + h32*v1 + h33);
    	 所以 p1通过 H21 投影到另一帧上 应该和p2的左边一致,但是实际不会一致,可以求得坐标误差
    	 误差平方和            squareDist2 = (u2-u2')*(u2-u2') + (v2-v2')*(v2-v2');
    	 使用方差倒数进行归一化 chiSquare2 = squareDist2*invSigmaSquare;
    
      使用阈值更新内外点标记 并记录内点得分:
    	if(chiSquare2>th)
    		bIn = false;              //距离大于阈值  该点 变换的效果差,记录为外点
    	else
    		score += th - chiSquare2; // 阈值 - 距离差值 得到 得分,差值越小  得分越高
    
      p2点 变成 p1点  p1 = H12 * p2 ------------------------------------
    	 |u1'|     |h11inv   h12inv   h13inv|    |u2|   |h11inv*u2 + h12inv*v2 + h13inv|
    	 |v1'|  =  |h21inv   h22inv   h23inv|  * |v2| = |h21inv*u2 + h22inv*v2 + h23inv|
    	 |1|       |h31inv   h32inv   h33inv|    |1|    |h31inv*u2 + h32inv*v2 + h33inv|
    	 使用第三行进行归一化: u1' = (h11inv*u2 + h12inv*v2 + h13inv)/(h31inv*u2 + h32inv*v2 + h33inv);
    			      v1' = (h21inv*u2 + h22inv*v2 + h23inv)/(h31inv*u2 + h32inv*v2 + h33inv);
    	 所以 p1通过 H21 投影到另一帧上 应该和p2的左边一致,但是实际不会一致,可以求得坐标误差
    	 误差平方和            squareDist1 = (u1-u1')*(u1-u1') + (v1-v1')*(v1-v1');
    	 使用方差倒数进行归一化 chiSquare1 = squareDist1*invSigmaSquare; 
    
      使用阈值更新内外点标记 并记录内点得分:
    	if(chiSquare1>th)    
    		bIn = false;             // 距离大于阈值  该点 变换的效果差,记录为外点
    	else
    		score += th - chiSquare1;// 阈值 - 距离差值 得到 得分,差值越小  得分越高  SH
    
      更新内外点记录数组:
    	if(bIn)
    		vbMatchesInliers[i]=true;// 是内点  误差较小
    	else
    		vbMatchesInliers[i]=false;// 是野点 误差较大
    

    从两个模型 H F 得分为 Sh Sf 中选着一个 最优秀的 模型 的方法为

    文中认为,当场景是一个平面、或近似为一个平面、或者视差较小的时候,可以使用单应性矩阵H恢复运动,
    当场景是一个非平面、视差大的场景时,使用基础矩阵F恢复运动,
    两个变换矩阵得分分别为 SH 、SF,
        根据两者得分计算一个评价指标RH:
    RH = SH / ( SH + SF)
    当大于0.45时,选择从单应性变换矩阵还原运动,反之使用基础矩阵恢复运动。
    不过ORB_SLAM2源代码中使用的是0.4作为阈值。
    

    c. 单应矩阵H恢复R,t Initializer::ReconstructH()

    单应矩阵恢复  旋转矩阵 R 和平移向量t
    p2   =  H21 * p1   
    p2 = K( RP + t)  = KTP = H21 * KP  
    A = T =  K 逆 * H21*K = [ R t; 0 0 0 1]
    对A进行奇异值分解
    cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
    使用FAUGERAS的论文[1]的方法,提出8种运动假设,分别可以得到8组R,t
    正常来说,第二帧能够看到的点都在相机的前方,即深度值Z大于0.
    但是如果在低视差的情况下,使用 三角变换Initializer::Triangulate() 得到的3d点云,
    使用 Initializer::CheckRT() 统计符合该R,t的内点数量。
    

    Initializer::Triangulate() 使用2d-2d点对 和 变换矩阵 R,t 三角变换恢复2d点对应的3d点

    Trianularization: 已知匹配特征点对{p1  p2} 和 
    各自相机投影矩阵{P1 P2}, P1 = K*[I 0], P2 = K*[R t], 尺寸为[3,4]
    估计三维点 X3D
         p1 = P1 * X3D
         p2 = P2 * X3D
    采用直接线性变换DLT的方法(将式子变换成A*X=0的形式后使用SVD奇异值分解求解线性方程组):
    对于 p1 = P1 * X3D: 方程两边 左边叉乘 p1,可使得式子为0
    p1叉乘P1*X3D = 0
    其叉乘矩阵为:
    叉乘矩阵 =  
    	  |0  -1  y|
    	  |1   0 -x| 
    	  |-y  x  0| 
    上述等式可写: 
      |0  -1  y|  |P1.row(0)|  
      |1   0 -x| *|P1.row(1)|* X3D = 0
      |-y  x  0|  |P1.row(2)|  
    
    对于第一行 |0  -1  y| 会与P的三行分别相乘 得到四个值 与齐次3d点坐标相乘得到 0
    有 (y * P1.row(2) - P1.row(1) ) * X3D = 0
    对于第二行 |1   0 -x|有:
    有 (x * P1.row(2) - P1.row(0) ) * X3D = 0
    得到两个约束,另外一个点 p2 = P2 * X3D,也可以得到两个式子:
    (y‘ * P2.row(2) - P2.row(1) ) * X3D = 0
    (x’ * P2.row(2) - P2.row(0) ) * X3D = 0
    写成 A*X = 0的形式有:
    A =(维度4*4)
    |y * P1.row(2) - P1.row(1) |
    |x * P1.row(2) - P1.row(0) |
    |y‘ * P2.row(2) - P2.row(1)|
    |x’ * P2.row(2) - P2.row(0)|
    对A进行奇异值分解求解X
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    x3D = vt.row(3).t();// vt的最后一列,为X的解
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);//  转换成非齐次坐标  归一化
    

    initializer::CheckRT() 计算R , t 的得分(内点数量)

    统计规则,使用 Initializer::Triangulate()  三角化的3d点,
    会有部分点云跑到相机的后面(Z<0),该部分点是噪点,需要剔除.
    另外一个准则是将上述符合条件的3d点分别重投影会前后两帧上,
    与之前的orb特征点对的坐标做差,误差平方超过阈值,剔除。
    统计剩余符合条件的点对数量。
    在8种假设中,记录得到最大内点数量 和 次大内点数量 
    当最大内点数量 远大于 次大内点数量( 有明显的差异性),
    该最大内点数量对应的R,t,具有较大的可靠性,返回该,R,t。
    

    d. 基础矩阵F恢复R,t Initializer::ReconstructF()

    计算 本质矩阵 E  =  K转置 * F  * K
    E = t叉乘 R
    奇异值分解 E= U C  V   , U 、V 为正交矩阵, C 为奇异值矩阵  C =  diag(1, 1, 0)
    从本质矩阵E 恢复 旋转矩阵R 和 平移向量t
    有四种假设 得到四种 R,t
    理论参考 Result 9.19 in Multiple View Geometry in Computer Vision
    使用 initializer::CheckRT() 计算R , t 的得分(内点数量)
    使用4中情况中得分最高的R,t 作为最终恢复的R,t
    

    e. 单目创建初始地图 CreateInitialMapMonocular();

    思想:
    	使用单目相机前两帧生成的3d点创建初始地图,
    	使用全局地图优化 Optimizer::GlobalBundleAdjustemnt(mpMap,20)优化地图点,
    	计算场景的深度中值(地图点深度的中位数),
    	使用平均逆深度归一化 两帧的平移变换量t,
    	使用 平均逆深度归一化地图点。
    步骤:
    步骤1:使用前两帧创建关键帧,并把这 两帧 关键帧 加入地图,加入关键帧数据库。
    步骤2:使用之前三角化得到的3d点创建 地图点。
    步骤3:地图点和每一帧的2d点对应起来,在关键帧中添加这种对应关系。 关键帧关联地图点。
    步骤4:关键帧和2d点对应起来,在地图点中添加这种关系。            地图点关联关键帧。
    步骤5:一个地图点会被许多个关键帧观测到,那么就会关联到许多个2d点,需要更新地图点对应2d点的orb特征描述子。
    步骤6:当前帧关联地图点,地图点加入到地图。
    步骤7:更新关键帧的 连接关系,被地图点观测到的次数。
    步骤8:全局优化地图 BA最小化重投影误差,这两帧姿态进行全局优化。
           Optimizer::GlobalBundleAdjustemnt(mpMap,20);
           // 注意这里使用的是全局优化,和回环检测调整后的大回环优化使用的是同一个函数。
           // 放在后面再进行解析
    步骤9:计算场景深度中值,以及逆深度中值,对位姿的平移量进行尺度归一化,对地图点进行尺度归一化。
    

    2. 双目/RGBD初始化 Tracking::StereoInitialization()根据视差计算深度(深度相机直接获取深度)计算3d点,创建地图点,创建初始地图

    步骤:
    	当前帧 特征点个数 大于500 进行初始化
    	设置第一帧为关键帧,并设置为位姿为 T = [I 0],世界坐标系 
    	创建关键帧,地图添加关键帧,
    	根据每一个2d点的视差(左右两张图像,orb特征点金字塔分层分块匹配得到视差d)求得的深度D=fB/d,计算对应的3D点,并创建地图点,
    	地图点关联观测帧 地图点计算所有关键帧中最好的描述子并更新地图点的方向和距离,
    	关键帧关联地图点,地当前帧添加地图点  地图添加地图点
    	局部地图中添加该初始关键帧。
    

    4.3 两帧跟踪得到初始化位姿(跟踪上一帧/跟踪参考帧/重定位)

    参考

    本文做匹配的过程中,是用的都是ORB特征描述子。
    先在8层图像金字塔中,提取FAST特征点。
    提取特征点的个数根据图像分辨率不同而不同,高分辨率的图像提取更多的角点。
    然后对检测到的特征点用ORB来描述,用于之后的匹配和识别。
    跟踪这部分主要用了几种模型:
    运动模型(Tracking with motion model)、
    关键帧(Tracking with reference keyframe)和
    重定位(Relocalization)。
    
    if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
    // 没有移动(跟踪参考关键帧的运动模型是空的)  或 刚完成重定位不久
    {
        bOK = TrackReferenceKeyFrame();// 跟踪参考帧模式
    }
    else
    {
        bOK = TrackWithMotionModel();// 跟踪上一帧模式
        if(!bOK)//没成功
            bOK = TrackReferenceKeyFrame();//再次使用 跟踪参考帧模式
    }
    if(!bOK)// 当前帧与最近邻关键帧的匹配也失败了,那么意味着需要重新定位才能继续跟踪。
    {       // 此时,只有去和所有关键帧匹配,看能否找到合适的位置。
        bOK = Relocalization();//重定位  BOW搜索,PnP 3d-2d匹配 求解位姿
    }
    
    

    a. Tracking::TrackWithMotionModel() 跟踪上一帧模式,两帧相差不大,对应位置区域领域半径搜索匹配点对

    参考:
    	这个模型是假设物体处于匀速运动,例如匀速运动的汽车、机器人、行人等,
    	就可以用上一帧的位姿和速度来估计当前帧的位姿。
    	使用的函数为 Tracking::TrackWithMotionModel()。
    	这里匹配是通过投影来与上一帧看到的地图点匹配,使用的是 
    	matcher.SearchByProjection()。
    
    思想:
    	移动模式跟踪  跟踪前后两帧  得到 变换矩阵。
    	上一帧的地图3d点反投影到当前帧图像像素坐标上,在不同尺度下不同的搜索半径内,
    	做描述子匹配 搜索 可以加快匹配。
    	在投影点附近根据描述子距离进行匹配(需要>20对匹配,否则匀速模型跟踪失败,
    	运动变化太大时会出现这种情况),然后以运动模型预测的位姿为初值,优化当前位姿,
    	优化完成后再剔除外点,若剩余的匹配依然>=10对,
    	则跟踪成功,否则跟踪失败,需要Relocalization:
    步骤:
        步骤1:创建 ORB特征点匹配器 最小距离 < 0.9*次小距离 匹配成功
    	   ORBmatcher matcher(0.9,true);
        步骤2:更新上一帧的位姿和地图点
    	   Tracking::UpdateLastFrame();
    	   上一帧位姿 = 上一帧到其参考帧位姿*其参考帧到世界坐标系(系统第一帧)位姿
    	   后面单目不执行
    	   双目或rgbd相机,根据深度值为上一帧产生新的MapPoints
        步骤3:使用当前的运动速度(之前前后两帧位姿变换)和上一帧的位姿来初始化 当前帧的位姿R,t
        步骤4:在当前帧和上一帧之间搜索匹配点(需要>20对匹配,否则匀速模型跟踪失败
    	   ORBmatcher::SearchByProjection(mCurrentFrame,mLastFrame,th,...)
    	   通过投影(使用当前帧的位姿R,t),对上一帧的特征点(地图点)进行跟踪.
    	   上一帧中包含了MapPoints,对这些MapPoints进行跟踪tracking,由此增加当前帧的MapPoints.
    	   上一帧3d点投影到当前坐标系下,在该2d点半径th范围内搜索可以匹配的匹配点,
    	   遍历可以匹配的点,计算描述子距离,记录最小的匹配距离,小于阈值的,再记录匹配点特征方向差值
    	   如果需要进行方向验证,剔除方向差直方图统计中,方向差值数量少的点对,保留前三个数量多的点对。
        步骤5:如果找到的匹配点对如果少于20,则扩大搜索半径th=2*th,使用 ORBmatcher::SearchByProjection()再次进行搜索。
        步骤6:使用匹配点对对当前帧的位姿进行优化 G2O图优化
    	   Optimizer::PoseOptimization(&mCurrentFrame);// 仅仅优化单个普通帧的位姿,地图点不优化
    	   输入前一帧3d点 和 当前帧2d点对,以及帧的初始化位姿Tcw
    	   3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw),只优化Frame的Tcw,不优化MapPoints的坐标。
    		1. 顶点 Vertex: g2o::VertexSE3Expmap(),初始值为当前帧的Tcw
    		2. 边 Edge:
    		       单目
    		     - g2o::EdgeSE3ProjectXYZOnlyPose(),一元边 BaseUnaryEdge
    			 + 顶点 Vertex:待优化当前帧的Tcw
    			 + 测量值 measurement:MapPoint在当前帧中的二维位置(u,v)
    			 + 误差信息矩阵 InfoMatrix: Eigen::Matrix2d::Identity()*invSigma2(与特征点所在的尺度有关)
    			 + 附加信息: 相机内参数: e->fx fy cx cy
    				     3d点坐标  : e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
    			双目 
    		     - g2o::EdgeStereoSE3ProjectXYZOnlyPose(),BaseUnaryEdge
    			 + Vertex:待优化当前帧的Tcw
    			 + measurement:MapPoint在当前帧中的二维位置(ul,v, ur) 左相机3d点 右相机横坐标匹配点坐标ur
    			 + InfoMatrix: invSigma2(与特征点所在的尺度有关)
    			 + 附加信息: 相机内参数: e->fx fy cx cy
    				     3d点坐标  : e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
    		 优化多次,根据误差,更新2d-3d匹配质量内外点标记,当前帧设置优化后的位姿。
        步骤7:如果2d-3d匹配效果差,被标记为外点,则当前帧2d点对于的3d点设置为空,留着以后再优化
        步骤8:根据内点的匹配数量,判断 跟踪上一帧是否成功。
    

    b. Tracking::TrackReferenceKeyFrame() 跟踪参考帧模式,bow特征向量加速匹配

    思想: 
    	当使用运动模式匹配到的特征点数较少时,就会选用关键帧模式。
    	即尝试和最近一个关键帧去做匹配。
    	为了快速匹配,本文利用了bag of words(BoW)来加速匹配。
    	首先,计算当前帧的BoW,并设定初始位姿为上一帧的位姿;
    	其次,根据两帧的BoW特征向量同属于一个node下来加速搜索匹配点对,使用函数matcher.SearchByBoW();
    	最后,利用匹配的特征优化位姿。
    
    步骤:
    	步骤1:创建 ORB特征点匹配器 最小距离 < 0.7*次小距离 匹配成功
    	      ORBmatcher matcher(0.7,true);
    	步骤2: 在当前帧 和 参考关键帧帧之间搜索匹配点对(需要>15对匹配点对,否者失败)
    	      ORBmatcher::SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
    	      计算当前帧 和 参考帧 orb特征的 词袋表示的 词典表示向量,属于同一个词典node下的点才可能是匹配点对
    	      遍历所有的参考帧特征点 和 当前帧特征点:
    	      同一个词典node下,有一些属于参考帧的点,也有一些属于当前帧的点,其中有一些匹配点。
    	      遍历属于一个node下的 参考帧的点
    		      遍历对应node下的当前帧的点
    			  计算orb描述子之间的距离
    			       记录最小的距离和次小的距离
    		      最小的距离小于阈值,且最小的距离明显小于次小的距离的话,
    		      记录次匹配点对特征的方向差,统计到方向差直方图
    	      如果需要根据方向一致性剔除不好的匹配点:
    		      计算方向差直方图中三个计数最多的方向差,
    		      根据数量差异选择保留第一/第一+第二/第一+第二+第三方向差对应的匹配点对。
                步骤3:设置当前帧位置为上一帧的位姿,使用步骤2得到的匹配点对3d-2d点对,使用BA 进行 当前帧位姿的优化
    	        Optimizer::PoseOptimization(&mCurrentFrame);
    		输入前一帧3d点 和 当前帧2d点对,以及帧的初始化位姿Tcw
    		3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw),只优化Frame的Tcw,不优化MapPoints的坐标。
    		1. 顶点 Vertex: g2o::VertexSE3Expmap(),初始值为当前帧的Tcw
    		2. 边 Edge:
    		       单目
    		     - g2o::EdgeSE3ProjectXYZOnlyPose(),一元边 BaseUnaryEdge
    			 + 顶点 Vertex:待优化当前帧的Tcw
    			 + 测量值 measurement:MapPoint在当前帧中的二维位置(u,v)
    			 + 误差信息矩阵 InfoMatrix: Eigen::Matrix2d::Identity()*invSigma2(与特征点所在的尺度有关)
    			 + 附加信息: 相机内参数: e->fx fy cx cy
    				     3d点坐标  : e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
    			双目 
    		     - g2o::EdgeStereoSE3ProjectXYZOnlyPose(),BaseUnaryEdge
    			 + Vertex:待优化当前帧的Tcw
    			 + measurement:MapPoint在当前帧中的二维位置(ul,v, ur) 左相机3d点 右相机横坐标匹配点坐标ur
    			 + InfoMatrix: invSigma2(与特征点所在的尺度有关)
    			 + 附加信息: 相机内参数: e->fx fy cx cy
    				     3d点坐标  : e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
    		 优化多次,根据误差,更新2d-3d匹配质量内外点标记,当前帧设置优化后的位姿。
            步骤4:如果2d-3d匹配效果差,被标记为外点,则当前帧2d点对于的3d点设置为空,留着以后再优化
            步骤5:根据内点的匹配数量,判断 跟踪上一帧是否成功。
    

    c. bool Tracking::Relocalization() 上面两种模式都没有跟踪成功,需要使用重定位模式,使用orb字典编码在关键帧数据库中找相似的关键帧,进行匹配跟踪

    思想:
        位置丢失后,需要在之前的关键帧中匹配最相近的关键帧,进而求出位姿信息。
        使用当前帧的BoW特征映射,在关键帧数据库中寻找相似的候选关键帧,因为这里没有好的初始位姿信息,
        需要使用传统的3D-2D匹配点的EPnP算法来求解一个初始位姿,之后再使用最小化重投影误差来优化更新位姿。
    步骤:
        步骤1:计算当前帧的BoW映射
    	   词典 N个M维的单词,一帧的描述子,n个M维的描述子,生成一个 N*1的向量,记录一帧的描述子使用词典单词的情况。 
        步骤2:在关键帧数据库中找到与当前帧相似的候选关键帧组,词典单词线性表示向量距离较近的一些关键帧。
        步骤3:创建 ORB特征点匹配器 最小距离 < 0.75*次小距离 匹配成功。
    	   ORBmatcher matcher(0.75,true);
        步骤4:创建 PnPsolver 位姿变换求解器数组,对应上面的多个候选关键帧。
        步骤5:遍历每一个候选关键帧使用BOW特征向量加速匹配,如果匹配点数少于15,跳过此候选关键帧。
    	    ORBmatcher::SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
    		      计算当前帧 和 参考帧 orb特征的 词袋表示的 词典表示向量,属于同一个词典node下的点才可能是匹配点对
    		      遍历所有的参考帧特征点 和 当前帧特征点:
    		      同一个词典node下,有一些属于参考帧的点,也有一些属于当前帧的点,其中有一些匹配点。
    		      遍历属于一个node下的 参考帧的点
    			      遍历对应node下的当前帧的点
    				  计算orb描述子之间的距离
    				       记录最小的距离和次小的距离
    			      最小的距离小于阈值,且最小的距离明显小于次小的距离的话,
    			      记录次匹配点对特征的方向差,统计到方向差直方图
    		      如果需要根据方向一致性剔除不好的匹配点:
    			      计算方向差直方图中三个计数最多的方向差,
    			      根据数量差异选择保留第一/第一+第二/第一+第二+第三方向差对应的匹配点对。
        步骤6:使用PnPsolver 位姿变换求解器,更加3d-2d匹配点,s1 * (u,v,1) = T * (X, Y, Z, 1)  = T * P
    	  6点直接线性变换DLT,后使用QR分解得到 R,t, 或者使用(P3P),3点平面匹配算法求解。
    	  这里可参考 文档--双目slam基础.md--里面的3d-2d匹配点对求解算法。
    	  这里会结合 Ransac 随采样序列一致性算法,来提高求解的鲁棒性。  
        步骤7:设置上面PnP算法求解的位置为当前帧的初始位姿,使用最小化重投影误差BA算法来优化位姿
    		Optimizer::PoseOptimization(&mCurrentFrame);
    		输入前一帧3d点 和 当前帧2d点对,以及帧的初始化位姿Tcw
    		3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw),只优化Frame的Tcw,不优化MapPoints的坐标。
    		1. 顶点 Vertex: g2o::VertexSE3Expmap(),初始值为当前帧的Tcw
    		2. 边 Edge:
    		       单目
    		     - g2o::EdgeSE3ProjectXYZOnlyPose(),一元边 BaseUnaryEdge
    			 + 顶点 Vertex:待优化当前帧的Tcw
    			 + 测量值 measurement:MapPoint在当前帧中的二维位置(u,v)
    			 + 误差信息矩阵 InfoMatrix: Eigen::Matrix2d::Identity()*invSigma2(与特征点所在的尺度有关)
    			 + 附加信息: 相机内参数: e->fx fy cx cy
    				     3d点坐标  : e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
    			双目 
    		     - g2o::EdgeStereoSE3ProjectXYZOnlyPose(),BaseUnaryEdge
    			 + Vertex:待优化当前帧的Tcw
    			 + measurement:MapPoint在当前帧中的二维位置(ul,v, ur) 左相机3d点 右相机横坐标匹配点坐标ur
    			 + InfoMatrix: invSigma2(与特征点所在的尺度有关)
    			 + 附加信息: 相机内参数: e->fx fy cx cy
    				     3d点坐标  : e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
    		 优化多次,根据误差,更新2d-3d匹配质量内外点标记,当前帧设置优化后的位姿。
        步骤8:如果优化时记录的匹配点对内点数量少于50,会把参考关键中还没有在当前帧有2d匹配的点反投影到当前帧下,再次搜索2d匹配点
    	  ORBmatcher::SearchByProjection();
        步骤9: 如果再次反投影搜索的匹配点数量和之前得到的匹配点数量 大于50,再次使用 Optimizer::PoseOptimization() 优化算法进行优化
        步骤10:如果上面优化后的内点数量还比较少可以再次搜索 matcher2.SearchByProjection(),再次优化  Optimizer::PoseOptimization()
        步骤11:如果内点数量 大于等于50 ,则重定位成功。
    

    4.4 Tracking::TrackLocalMap() 跟踪局部地图,多帧局部地图G2O优化位姿, 新建关键帧

    参考

    思想:
    	上面完成初始位姿的跟踪后,需要 使用局部地图(参考帧的一二级共视帧组成) 来 进行局部地图优化,来提高鲁棒性。
    	局部地图中与当前帧有相同点的关键帧序列成为一级相关帧K1,
    	而与一级相关帧K1有共视地图点的关键帧序列成为二级相关帧K2,
    	把局部地图中的局部地图点,投影到当前帧上,如果在当前帧的视野内
    	使用 位姿优化 Optimizer::PoseOptimization(&mCurrentFrame), 进行优化,
    	更新 地图点的信息(关键帧的观测关系)
    步骤:
         步骤1:首先对局部地图进行更新(UpdateLocalMap) 生成对应当前帧的局部地图(小图)
               Tracking::UpdateLocalMap();
    	     ---更新局部关键帧:Tracking::UpdateLocalKeyFrames();
    	             始终限制局部关键帧(小图中关键帧的数量)数量不超过80
    		     包含三个部分:
    		       1. 共视化程度高的关键帧: 观测到当前帧的地图点次数多的关键帧;
    		       2. 这些关键帧的孩子关键帧;(这里没看到具体的方式,应该就是根据时间顺序记录了一些父子关键帧)
    		       3. 这些关键帧的父亲关键帧。
    	     ---更新局部地图点:Tracking::UpdateLocalPoints();
    	            所有局部关键帧 包含的地图点构成 局部地图点
    		    遍历每一个局部关键帧的每一个地图点:
    		          加入到局部地图点中: mvpLocalMapPoints.push_back(pMP);
    			  同时设置地图点更新标志,来避免重复添加出现在多帧上的地图点。
         步骤2:在局部地图中为当前帧搜索匹配点对
               Tracking::SearchLocalPoints();// 在对应当前帧的局部地图内搜寻和 当前帧地图点匹配点的 局部地图点
               1. 遍历当前帧的特征点,如果已经有相应的3D地图点,则进行标记,不需要进行重投影匹配,并且标记已经被遍历过
               2. 遍历局部地图的所有地图点,如果没有被遍历过,把地图点反投影到当前帧下,保留在当前帧视野内的地图点
               3. 根据反投影后的2d位置,设置一个半径为th的范围进行搜索匹配点
                  SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
    	      遍历可以匹配的点,计算描述子距离,记录最小的匹配距离,小于阈值的,再记录匹配点特征方向差值
    	      如果需要进行方向验证,剔除方向差直方图统计中,方向差值数量少的点对,保留前三个数量多的点对。
         步骤3:使用之前得到的初始位姿和 在局部地图中搜索到的3d-2d匹配点对,使用最小化重投影误差BA算法来优化位姿
                Optimizer::PoseOptimization(&mCurrentFrame);
    	    输入前一帧3d点 和 当前帧2d点对,以及帧的初始化位姿Tcw
    		3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw),只优化Frame的Tcw,不优化MapPoints的坐标。
    		1. 顶点 Vertex: g2o::VertexSE3Expmap(),初始值为当前帧的Tcw
    		2. 边 Edge:
    		       单目
    		     - g2o::EdgeSE3ProjectXYZOnlyPose(),一元边 BaseUnaryEdge
    			 + 顶点 Vertex:待优化当前帧的Tcw
    			 + 测量值 measurement:MapPoint在当前帧中的二维位置(u,v)
    			 + 误差信息矩阵 InfoMatrix: Eigen::Matrix2d::Identity()*invSigma2(与特征点所在的尺度有关)
    			 + 附加信息: 相机内参数: e->fx fy cx cy
    				     3d点坐标  : e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
    			双目 
    		     - g2o::EdgeStereoSE3ProjectXYZOnlyPose(),BaseUnaryEdge
    			 + Vertex:待优化当前帧的Tcw
    			 + measurement:MapPoint在当前帧中的二维位置(ul,v, ur) 左相机3d点 右相机横坐标匹配点坐标ur
    			 + InfoMatrix: invSigma2(与特征点所在的尺度有关)
    			 + 附加信息: 相机内参数: e->fx fy cx cy
    				     3d点坐标  : e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
    		 优化多次,根据误差,更新2d-3d匹配质量内外点标记,当前帧设置优化后的位姿。
         步骤4:更新地图点状态
         步骤5:如果刚刚进行过重定位 则需要 内点匹配点对数 大于50 才认为 成功
               正常情况下,找到的内点匹配点对数 大于30 算成功
    

    4.5 跟踪成功之后判断是否需要新建关键帧

    判断是否需要创建关键帧 Tracking::NeedNewKeyFrame();

    确定关键帧的标准如下:
    	(1)在上一个全局重定位后,又过了20帧               (时间过了许久);
    	(2)局部建图闲置,或在上一个关键帧插入后,又过了20帧(时间过了许久);
    	(3) 当前帧跟踪到点数量比较少,tracking质量较弱     (跟踪要跪的节奏);
    	(4)当前帧跟踪到的点比参考关键帧的点少90%          (环境变化较大了)。
    步骤:
         步骤1:系统模式判断,如果仅仅需要跟踪定位,不需要建图,那么不需要新建关键帧。
         步骤2:根据地图中关键帧的数量设置一些参数(系统一开始关键帧少的时候,可以放宽一些条件,多创建一些关键帧)
         步骤3:很长时间没有插入关键帧 
    	    bool c1a = mCurrentFrame.mnId >= mnLastKeyFrameId + mMaxFrames;
         步骤4:在过去一段时间但是局部建图闲置
    	    bool c1b = (mCurrentFrame.mnId >= mnLastKeyFrameId+mMinFrames && bLocalMappingIdle
         步骤5:当前帧跟踪到点数量比较少,tracking质量较弱
    	    bool c1c = mnMatchesInliers < nRefMatches*0.25
         步骤6:上面条件成立之前必须当 当前帧与之前参考帧(最近的一个关键帧)重复度不是太高。
    	    bool c2 = ((mnMatchesInliers < nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15)
         步骤7:需要新建关键帧的条件 (c1a || c1b || c1c) && c2
         步骤8:当待插入的关键帧队列里关键帧数量不多时在插入,队列里不能阻塞太多关键帧。
    

    创建关键帧 Tracking::CreateNewKeyFrame();

    步骤:
         步骤1:将当前帧构造成关键帧
         步骤2:将当前关键帧设置为当前帧的参考关键帧
         步骤3:对于双目或rgbd摄像头,为当前帧生成新的MapPoints
    	   将深度距离比较近的点包装成MapPoints
    	     这些添加属性的操作是每次创建MapPoint后都要做的:
    	   地图点关键关键帧
    	   关键帧关联地图点
    	   地图点更新最优区别性的描述子
    	   地图点更新深度
    	   地图添加地图点
    

    局部建图线程 LocalMapping::LocalMapping()

    功能总览:
    	LocalMapping作用是将Tracking中送来的关键帧放在mlNewKeyFrame列表中;
    	处理新关键帧,地图点检查剔除,生成新地图点,Local BA,关键帧剔除。
    	主要工作在于维护局部地图,也就是SLAM中的Mapping。
    	Tracking线程 只是判断当前帧是否需要加入关键帧,并没有真的加入地图,
    	因为Tracking线程的主要功能是局部定位, 而处理地图中的关键帧,地图点,包括如何加入,
    	如何删除的工作是在LocalMapping线程完成的
    
    void LocalMapping::Run():
    	局部建图 :处理新的关键帧 KeyFrame 完成局部地图构建,
    	插入关键帧 ------>  
    	处理地图点(筛选生成的地图点 生成地图点)  -------->  
    	局部 BA最小化重投影误差  -调整-------->  
    	筛选 新插入的 关键帧
    
    步骤:
    	mlNewKeyFrames     list 列表队列存储关键帧
    	步骤1:设置进程间的访问标志 告诉Tracking线程,LocalMapping线程正在处理新的关键帧,处于繁忙状态
    	步骤2:检查队列  LocalMapping::CheckNewKeyFrames(); 等待处理的关键帧列表不为空
    	步骤3:处理新关键帧,计算关键帧特征点的词典单词向量BoW映射,将关键帧插入地图  
    	      更新地图点MapPoints 和 关键帧 KepFrame 的关联关系  UpdateConnections() 更新关联关系
    	      LocalMapping::ProcessNewKeyFrame();
    	步骤4:创建新的地图点 相机运动过程中与相邻关键帧通过三角化恢复出一些新的地图点MapPoints
    	      LocalMapping::CreateNewMapPoints();
    	步骤5:对新添加的地图点进行融合处理 ,剔除 地图点 MapPoints
    	      对于 ProcessNewKeyFrame 和 CreateNewMapPoints 中 最近添加的MapPoints进行检查剔除
    	      删除地图中新添加的但 质量不好的 地图点
    	      LocalMapping::MapPointCulling();
    	      a)  IncreaseFound / IncreaseVisible < 25%
    	      b) 观测到该 点的关键帧太少
    	步骤6:相邻帧地图点融合  
    	      LocalMapping::SearchInNeighbors();
    	      检测当前关键帧和相邻 关键帧(两级相邻) 中 重复的 地图点 
    
    	步骤7:局部地图BA 最小化重投影误差
    	      Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
    	      和当前关键帧相邻的关键帧 中相匹配的 地图点对 最局部 BA最小化重投影误差优化点坐标 和 位姿
    
    	步骤7:关键帧融合,剔除当前帧相邻的关键帧中冗余的关键帧
    	      LocalMapping::KeyFrameCulling();
    	      其90%以上的 地图点 能够被其他 共视 关键帧(至少3个) 观测到,认为该关键帧多余,可以删除
    	步骤8:将当前帧加入到闭环检测队列中 mpLoopCloser
    	步骤9:等待线程空闲 完成一帧关键帧的插入融合工作
    	步骤10:告诉 Tracking 线程,Local Mapping 线程现在空闲,可一处理接收下一个关键帧。
    

    恢复3d点 LocalMapping::CreateNewMapPoints()

    思想:
    	相机运动过程中和共视程度比较高的关键帧,通过三角化恢复出一些MapPoints地图点
    	根据当前关键帧恢复出一些新的地图点,不包括和当前关键帧匹配的局部地图点(已经在ProcessNewKeyFrame中处理)
    	先处理新关键帧与局部地图点之间的关系,然后对局部地图点进行检查,
    	最后再通过新关键帧恢复 新的局部地图点:CreateNewMapPoints()
    步骤:
       步骤1:在当前关键帧的 共视关键帧 中找到 共视程度 最高的前nn帧 相邻帧vpNeighKFs
       步骤2:遍历和当前关键帧 相邻的 每一个关键帧vpNeighKFs
       步骤3:判断相机运动的基线在(两针间的相机相对坐标)是不是足够长
       步骤4:根据两个关键帧的位姿计算它们之间的基本矩阵 F =  inv(K1 转置) * t12 叉乘 R12 * inv(K2)
       步骤5:通过帧间词典向量加速匹配,极线约束限制匹配时的搜索范围,进行特征点匹配
       步骤6:对每对匹配点 2d-2d 通过三角化生成3D点,和 Triangulate函数差不多
    	步骤6.1:取出匹配特征点
    	步骤6.2:利用匹配点反投影得到视差角   
    		用来决定使用三角化恢复(视差角较大) 还是 直接2d点反投影(视差角较小)
    	步骤6.3:对于双目,利用双目基线 深度 得到视差角
    	步骤6.4:视差角较大时 使用 三角化恢复3D点 (两个点,四个方程,奇异值分解求解)
    	步骤6.4:对于双目 视差角较小时, 2d点利用深度值 反投影 成三维点 ,单目的话直接跳过
    	步骤6.5:检测生成的3D点是否在相机前方(Z>0)
    	步骤6.6:计算3D点在当前关键帧下的重投影误差,误差较大的跳过
    	步骤6.7:计算3D点在邻接关键帧 下的重投影误差,误差较大的跳过
    	步骤6.9:三角化生成3D点成功,构造成地图点 MapPoint
    	步骤6.9:为该MapPoint添加属性 
    			地图点关键关键帧
    			关键帧关联地图点
    			地图点更新最优区别性的描述子
    			地图点更新深度
    			地图添加地图点   
    	步骤6.10:将新产生的点放入检测队列 mlpRecentAddedMapPoints  交给 MapPointCulling() 检查生成的点是否合适
    

    4.7 闭环检测线程 LoopClosing

    参考
    思想:
    闭环检测线程
    对新加入的关键帧,
    1. 进行回环检测(WOB 二进制词典匹配检测,通过Sim3算法计算相似变换)--------->
    2. 闭环校正(闭环融合 和 图优化)

    LoopClosing::Run() 闭环检测步骤:
       mlpLoopKeyFrameQueue  闭环检测关键帧队列(localMapping线程 加入的)
       只要闭环检测关键帧队列不为空下面的检测会一直执行
    步骤0:进行闭环检测 LoopClosing::DetectLoop()
    	步骤1:从队列中抽取一帧 
    	      mpCurrentKF = mlpLoopKeyFrameQueue.front();
    	      mlpLoopKeyFrameQueue.pop_front();//出队
    	步骤2:判断距离上次闭环检测是否超过10帧,如果数量过少则不进行闭环检测。
    	步骤3:遍历所有共视关键帧,计算当前关键帧与每个共视关键帧 的bow相似度得分,并得到最低得分minScore 
    	      GetVectorCovisibleKeyFrames(); // 当前帧的所有 共视关键帧
    	      mpORBVocabulary->score(CurrentBowVec, BowVec);// bow向量相似性得分
    	步骤4:在所有关键帧数据库中找出与当前帧按最低得分minScore 匹配得到的闭环候选帧 vpCandidateKFs
    	步骤5:在候选帧中检测具有连续性的候选帧,相邻一起的分成一组,组与组相邻的再成组(pKF, minScore)  
    	步骤6:找出与当前帧有 公共单词的 非相连 关键帧 lKFsharingwords
    	步骤7:统计候选帧中 与 pKF 具有共同单词最多的单词数 maxcommonwords
    	步骤8:得到阈值 minCommons = 0.8 × maxcommonwords  
    	步骤9:筛选共有单词大于 minCommons  且词带 BOW 匹配 最低得分  大于  minScore , lscoreAndMatch
    	步骤10:将存在相连的分为一组,计算组最高得分 bestAccScore,同时得到每组中得分最高的关键帧  lsAccScoreAndMatch
    	步骤11:得到阈值 minScoreToRetain = 0.75  ×  bestAccScore  
    	步骤12:得到 闭环检测候选帧
    步骤13:计算 闭环处两针的 相似变换  [sR|t]
    	LoopClosing::ComputeSim3()
    步骤14:闭环融合位姿图优化
           LoopClosing::CorrectLoop()
    

    LoopClosing::ComputeSim3() 计算当前帧与闭环帧的Sim3变换

    思想:
        通过Bow词袋量化加速描述子的匹配,利用RANSAC粗略地计算出当前帧与闭环帧的Sim3(当前帧---闭环帧)
        根据估计的Sim3,对3D点进行投影找到更多匹配,通过优化的方法计算更精确的Sim3(当前帧---闭环帧)
        将闭环帧以及闭环帧相连的关键帧的MapPoints与当前帧的点进行匹配(当前帧---闭环帧+相连关键帧)
        注意以上匹配的结果均都存在成员变量 mvpCurrentMatchedPoints 中,
        实际的更新步骤见CorrectLoop()步骤3:Start Loop Fusion
    步骤:
        步骤1:遍历每一个闭环候选关键帧  构造 sim3 求解器
        步骤2:从筛选的闭环候选帧中取出一帧关键帧pKF
        步骤3:将当前帧mpCurrentKF与闭环候选关键帧pKF匹配 得到匹配点对
    	  步骤3.1 跳过匹配点对数少的 候选闭环帧
    	  步骤3.2:  根据匹配点对 构造Sim3求解器
        步骤4:迭代每一个候选闭环关键帧  Sim3利用 相似变换求解器求解 候选闭环关键帧到档期帧的 相似变换
        步骤5:通过步骤4求取的Sim3变换,使用sim3变换匹配得到更多的匹配点 弥补步骤3中的漏匹配
        步骤6:G2O Sim3优化,只要有一个候选帧通过Sim3的求解与优化,就跳出停止对其它候选帧的判断
        步骤7:如果没有一个闭环匹配候选帧通过Sim3的求解与优化 清空候选闭环关键帧
        步骤8:取出闭环匹配上 关键帧的相连关键帧,得到它们的地图点MapPoints放入mvpLoopMapPoints
        步骤9:将闭环匹配上关键帧以及相连关键帧的 地图点 MapPoints 投影到当前关键帧进行投影匹配 为当前帧查找更多的匹配
        步骤10:判断当前帧 与检测出的所有闭环关键帧是否有足够多的MapPoints匹配	
        步骤11:满足匹配点对数>40 寻找成功 清空mvpEnoughConsistentCandidates
    

    LoopClosing::CorrectLoop() 闭环融合 全局优化

    步骤: 
        步骤1:通过求解的Sim3以及相对姿态关系,调整与 当前帧相连的关键帧 mvpCurrentConnectedKFs 位姿 
    	  以及这些 关键帧观测到的MapPoints的位置(相连关键帧---当前帧)
        步骤2:用当前帧在闭环地图点 mvpLoopMapPoints 中匹配的 当前帧闭环匹配地图点 mvpCurrentMatchedPoints  
    	  更新当前帧 之前的 匹配地图点 mpCurrentKF->GetMapPoint(i)
        步骤3:将闭环帧以及闭环帧相连的关键帧的 所有地图点 mvpLoopMapPoints 和  当前帧相连的关键帧的点进行匹配 
        步骤4:通过MapPoints的匹配关系更新这些帧之间的连接关系,即更新covisibility graph
        步骤5:对Essential Graph(Pose Graph)进行优化,MapPoints的位置则根据优化后的位姿做相对应的调整
       步骤6:创建线程进行全局Bundle Adjustment
    
    mvpCurrentConnectedKFs    当前帧相关联的关键帧
    vpLoopConnectedKFs        闭环帧相关联的关键帧  这些关键帧的地图点 闭环地图点 mvpLoopMapPoints
    mpMatchedKF               与当前帧匹配的  闭环帧
    mpCurrentKF 当前关键帧     优化的位姿 mg2oScw     原先的地图点 mpCurrentKF->GetMapPoint(i)
    mvpCurrentMatchedPoints   当前帧在闭环地图点中匹配的地图点  当前帧闭环匹配地图点 
    

    5. 数学理论总结

    参考

    在这里插入图片描述

    展开全文
  • 双目相机

    千次阅读 2019-05-06 14:47:27
    双目相机 计算机视觉基础1——视差与深度信息 目录 1. 单目 2. 双目 3. RGBD 双目立体视觉 相机标定————畸变矫正————立体校正————极线约束————立体匹配(关键步骤)————三角测量 ...

     

    双目相机

     

    计算机视觉基础1——视差与深度信息

    目录

    1. 单目
    2. 双目
    3. RGBD
    

    双目立体视觉

    相机标定————畸变矫正————立体校正————极线约束————立体匹配(关键步骤)————三角测量
    

    立体匹配最基本的步骤:

    1)代价计算。
       计算左图一个像素和右图一个像素之间的代价(某种差值)。
       
    2)代价聚合。
       一般基于点之间的匹配很容易受噪声的影响,往往真实匹配的像素的代价并不是最低。
       所以有必要在点的周围建立一个window,让像素块和像素块之间进行比较,这样肯定靠谱些。
       代价聚合往往是局部算法或者半全局算法才会使用,
       全局算法抛弃了window,采用基于全图信息的方式建立能量函数。
       
    3)深度赋值。
       这一步可以区分局部算法与全局算法,局部算法直接优化代价聚合模型。
       而全局算法,要建立一个能量函数,能量函数的数据项往往就是代价聚合公式,例如DoubleBP。
       输出的是一个粗略的视差图。
       
    4)结果优化。
       对上一步得到的粗估计的视差图进行精确计算,策略有很多,
       例如plane fitting,BP,动态规划等。
    

    局部方法

    Winner-takes-All
    赢者通吃————先入为主—————用户黏性
    局部最小
    

    方法有:

    1. Block Matching 块匹配
    
    
    6. CSCA
    

    半全局

    全局

    双目 ros 包 elas

    wiki.ros elas_ros

    github code cyphy-elas-ros

    普通工程 ELAS

    stereo_elas

    双目障碍物检测

    深度学习双目匹配 DispNet

    可以建立 相机坐标系下的 点云

    如需建立 世界坐标系下的点云 需要跟踪 每一帧图像的位姿变化 既需要配合 视觉里程计来使用

    viso2

    机器人视觉 实验室 代码参考 

    机器人视觉 实验室 代码参考 

    elas使用

    节点 Nodes

    节点1. elas_ros Nodes

    完成 双目视差匹配disparity matching, 生成当前相机坐标系下的点云point clouds   camera's local reference frame.

    订阅的话题 Subscribed Topics

    a. <stereo>/left/<image> (sensor_msgs/Image)      左图矫正后的图像 Left rectified stereo image. 
    
    b. <stereo>/right/<image> (sensor_msgs/Image)     右图矫正后的图像 Right rectified stereo image. 
    
    c. <stereo>/left/camera_info (sensor_msgs/CameraInfo) 左相机参数 Camera info for left camera. 
    
    4. <stereo>/right/camera_info (sensor_msgs/CameraInfo) 右相机参数 Camera info for right camera. 
    

    发布的话题 Published Topics

    a. disparity (sensor_msgs/Image)  视差 话题
       Image representing the calculated disparity at each point. 
        Note that the values are scaled to be in the range [0, 255] 
    
    b. point_cloud (sensor_msgs/PointCloud2) 左相机当前坐标系下 的点云地图
        The point cloud assosciated with the calculated disparity in the left camera's local reference frame. 
    
    c. frame_data (elas_ros/ElasFrameData)  结构数据  用于三维重建
        Frame data used for the point cloud concatenation process. See node pc_construction. 
    

    参数 Parameters

    a. ~queue_size (int, default: 5)  队列大小
        Length of the input queues for left and right camera synchronization. 
    
    b. ~approximate_sync (bool, default: false)  同步
        Whether the node should use approximate synchronization of incoming messages. 
        Set to true if cameras do not have synchronised timestamps. 
    

    节点2. pc_construction 三维重建

    This node concatenates the point clouds generated by the elas_ros node.
    

    订阅的话题 Subscribed Topics

    <frame_data> (elas_ros/ElasFrameData)   结构数据  用于三维重建
        The frame data generated by the elas_ros node. 
    
    <pose> (geometry_msgs/PoseStamped)    相机姿态数据  来自视觉里程计
        The pose transformation from base frame to pose frame. 
        This accounts for motion of the camera. This can typically be determined by appropriate visual odometry. 
        See usage notes below. 
    

    发布的话题Published Topics

    point_cloud (sensor_msgs/PointCloud2) 世界坐标系下的点云地图
        The concatenated point cloud. 
    

    参数 Parameters

    ~queue_size (int, default: 5)  队列大小 
        Length of the input queues for frame data and pose synchronization. 
    

      ~approximate_sync (bool, default: false)  同步 Whether the node should use approximate synchronization of incoming messages. Set to true if frame data and pose do not have synchronised timestamps.

    base_frame_id (string)  基坐标系
        The name of the base frame that the concatenated point cloud should be cast into. 
    
    pose_frame_id (string)  相机位姿坐标系
        The name of the frame that the given pose transforms into. 
    

    相机 图像处理流水线 ros包

    image_pipeline

    Tables Are Cool
    col 3 is right-aligned $1600
    col 2 is centered $12
    zebra stripes are neat $1

    显示链接中带括号的图片

    ![][1] [1]: http://latex.codecogs.com/gif.latex?\prod%20(n_{i})+1

    单目 双目相机 实战

    单目标定

    双目标定

    双目匹配

    OpenCV提供了以下四种立体匹配算法的函数:

    1. 基于局部的块匹配    Block Matching(BM) StereoBM
    2. 半全局块匹配        Semi-Global Block Matching(SGBM) StereoSGBM
    >第一步对每一个Pixel使用块匹配BM进行匹配,得到了全部Pixel的disparity map。
    >第二步对Disparity map建立图,用Graph Cut对其进行全局优化。
    >利用Rectification将二维转化为一维
    3. 基于全局的图割      Graph Cut(GC)cvStereoGCState()
    4. 基于全局的 动态规划 Dynamic Programming(DP)cvFindStereoCorrespondence()
    

    双目相机 算法

    ADCensus, SGBM, BM算法参考

    ELAS论文解析

    ELAS代码

    棋盘格

    双目相机 矫正

    * 用法 ./Stereo_Calibr -w=6 -h=8  -s=24.3 stereo_calib.xml   
      我的  ./Stereo_Calibr -w=8 -h=10 -s=200 stereo_calib.xml
    * ./stereo_calib -w=9 -h=6 stereo_calib.xml 标准图像
    *  实际的正方格尺寸在程序中指定 const float squareSize = 2.43f;    
       2.43cm  mm为单位的话为 24.3  0.1mm为精度的话为   243 注意 标定结果单位(纲量)和此一致
    

    原理分析 原理分析2

    径向畸变矫正 光学透镜特效 凸起 k1 k2 k3 三个参数确定

     Xp=Xd(1 + k1*r^2 + k2*r^4 + k3*r^6)
     Yp=Yd(1 + k1*r^2 + k2*r^4 + k3*r^6)
    

    切向畸变矫正 装配误差 p1 p2 两个参数确定

     Xp= Xd + ( 2*p1*y  + p2*(r^2 + 2*x^2) )
     Yp= Yd + ( p1 * (r^2 + 2*y^2) + 2*p2*x )
     r^2 = x^2+y^2
    

    投影公式 世界坐标点 到 相机像素坐标系下

                                                  | Xw|
    *  | u|     |fx  0   ux 0|     |    R   T  |    | Yw|
    *  | v| =   |0   fy  uy 0|  *  |           | *  | Zw| = M*W
    *  | 1|     |0   0  1   0|     |   0 0  0 1|    | 1 |
    

    相机标定和三维重建

    *   像素坐标齐次表示(3*1)  =  内参数矩阵 齐次表示(3*4)  ×  外参数矩阵齐次表示(4*4) ×  物体世界坐标 齐次表示(4*1)
    *   内参数齐次 × 外参数齐次 整合 得到 投影矩阵  3*4    左右两个相机 投影矩阵 P1 = K1*T1   P2 = k2*T2
    *   世界坐标 W  ----> 左相机投影矩阵 P1 ------> 左相机像素点 (u1,v1,1)
    *               ----> 右相机投影矩阵 P2 ------> 右相机像素点 (u2,v2,1)
    

    Q为 视差转深度矩阵 disparity-to-depth mapping matrix

    * Z = f*B/d       =   f    /(d/B)
    * X = Z*(x-c_x)/f = (x-c_x)/(d/B)
    * X = Z*(y-c_y)/f = (y-y_x)/(d/B)
    

    Q为 视差转深度矩阵 参考

    *    Q= | 1   0    0         -c_x     |    Q03
    *       | 0   1    0         -c_y     |    Q13
    *       | 0   0    0          f       |    Q23
    *       | 0   0   -1/B   (c_x-c_x')/B |    c_x和c_x' 为左右相机 平面坐标中心的差值(内参数)
    *                   Q32        Q33
    
    *  以左相机光心为世界坐标系原点   左手坐标系Z  垂直向后指向 相机平面  
    *           |x|      | x-c_x           |     |X|
    *           |y|      | y-c_y           |     |Y|
    *   Q    *  |d| =    |   f             |  =  |Z|======>  Z'  =   Z/W =     f/((-d+c_x-c_x')/B)
    *           |1|      |(-d+c_x-c_x')/B  |     |W|         X'  =   X/W = ( x-c_x)/((-d+c_x-c_x')/B)
                                                             Y'  =   Y/W = ( y-c_y)/((-d+c_x-c_x')/B)
                                                            与实际值相差一个负号
    

    双目 视差与深度的关系

    Z = f * T / D   
    f 焦距 量纲为像素点  
    T 左右相机基线长度 
    量纲和标定时 所给标定板尺寸 相同 
    D视差 量纲也为 像素点 分子分母约去,
    Z的量纲同 T
    

    像素单位与 实际物理尺寸的关系

    * CCD的尺寸是8mm X 6mm,帧画面的分辨率设置为640X480,那么毫米与像素点之间的转换关系就是80pixel/mm
    * CCD传感器每个像素点的物理大小为dx*dy,相应地,就有 dx=dy=1/80
    * 假设像素点的大小为k x l,其中 fx = f / k, fy = f / (l * sinA), 
      A一般假设为 90°,是指摄像头坐标系的偏斜度(就是镜头坐标和CCD是否垂直)。
    * 摄像头矩阵(内参)的目的是把图像的点从图像坐标转换成实际物理的三维坐标。因此其中的fx,y, cx, cy 都是使用类似上面的纲量。
    * 同样,Q 中的变量 f,cx, cy 也应该是一样的。
    
        参考代码 
        https://github.com/yuhuazou/StereoVision/blob/master/StereoVision/StereoMatch.cpp
        https://blog.csdn.net/hujingshuang/article/details/47759579
    
        http://blog.sina.com.cn/s/blog_c3db2f830101fp2l.html
    

    视差快匹配代价函数:

     【1】对应像素差的绝对值和(SAD, Sum of Absolute Differences) 
    
     【2】对应像素差的平方和(SSD, Sum of Squared Differences)
    
     【3】图像的相关性(NCC, Normalized Cross Correlation) 归一化积相关算法
    
     【4】ADCensus 代价

     

     

    ==============================================

    双目匹配

    手工设计的特征   匹配代价  

    1. 像素差的绝对值(SAD, Sum of Absolute Differences) 
    2. 像素差的平方和(SSD, Sum of Squared Differences)
    3. 图像的相关性(NCC, Normalized Cross Correlation) 
    4. Census 局部空间结构 汉明距离 匹配代价 
    5. AD + Census
    6. SD + Census
    7. ...
    

    卷积网络学习的特征 匹配代价

    Stereo Matching by Training a Convolutional Neural Network to Compare Image Patches 
    

    卷积网络双目匹配

    输入:两个图像块
    输出:匹配代价 Match Cost
    

    Network I

    two img --->two CNN ----> 连接(concatenate)----> 全连接 ----->相似性得分(similarity score)
    
    Small patch size 
    “Big” network(~600K) 
    Binary prediction
    

    Network II

    two img --->two CNN ---->标准化(normalizer)----->点乘(dot product) ----->相似性得分(similarity score)
    
    Dot-product 
    Small network
    Hinge loss
    

    Network III

    two img --->two CNN(共享权重)---------->相关性----->
    Full content 
    Dot-product 
    Larger patch 
    Log loss
    

    Dataset 数据集 Stereo Datasets

    middlebury数据集

    Middlebury 数据集

    KITTI Vision Benchmark Suite数据集

    KITTI 数据集

    训练

    1. 预处理,数据增强  Preprocessing, data-augmentation
    2. 网络:  梯度聚合  network: gradient aggregated 
    3. SGD;  Batch Normalization 批量化 
    

    Refinement in practice Smoothing 平滑

    代价聚合  Cost-aggregation 
        1.平均邻近位置 Averaging neighboring locations
        2.奇特的“邻居” 外点(遮挡点+不稳定)
    全局能量函数
    SGM(Stereo Processing by Semi-Global Matching and Mutual Information) 
    
    CRF  
    dynamic programming
    
    Border fixing(CNN) 
    Left-right consistency 
    Further smooth 
    Outlier detector
    

    三角测量

    深度与视差成反比 Depth is inversely proportional to disparity

    Z = fB/d 
      z:深度
      f:相机焦距
      d:像素点视差
    Y = (u - cy)*Z/f
    X = (v - cx)*Z/f
    展开全文

空空如也

空空如也

1 2 3 4 5
收藏数 96
精华内容 38
热门标签
关键字:

双目相机rgbd