Repository: datawhalechina/smoothly-vslam Branch: main Commit: b267b9fc6260 Files: 58 Total size: 344.1 KB Directory structure: gitextract_pi78t8bo/ ├── LICENSE ├── README.md ├── TASK.md ├── code/ │ ├── ch10/ │ │ ├── CMakeLists.txt │ │ ├── cmake_modules/ │ │ │ └── FindDBoW3.cmake │ │ ├── feature_training.cpp │ │ ├── gen_vocab_large.cpp │ │ └── loop_closure.cpp │ ├── ch11/ │ │ └── dense_mono/ │ │ ├── CMakeLists.txt │ │ └── dense_mapping.cpp │ └── ch12/ │ └── myslam/ │ ├── CMakeLists.txt │ ├── app/ │ │ ├── CMakeLists.txt │ │ └── run_kitti_stereo.cpp │ ├── bin/ │ │ └── run_kitti_stereo │ ├── cmake_modules/ │ │ ├── FindCSparse.cmake │ │ ├── FindG2O.cmake │ │ └── FindGlog.cmake │ ├── config/ │ │ └── default.yaml │ ├── include/ │ │ └── myslam/ │ │ ├── algorithm.h │ │ ├── backend.h │ │ ├── camera.h │ │ ├── common_include.h │ │ ├── config.h │ │ ├── dataset.h │ │ ├── feature.h │ │ ├── frame.h │ │ ├── frontend.h │ │ ├── g2o_types.h │ │ ├── map.h │ │ ├── mappoint.h │ │ ├── viewer.h │ │ └── visual_odometry.h │ └── src/ │ ├── CMakeLists.txt │ ├── backend.cpp │ ├── camera.cpp │ ├── config.cpp │ ├── dataset.cpp │ ├── feature.cpp │ ├── frame.cpp │ ├── frontend.cpp │ ├── map.cpp │ ├── mappoint.cpp │ ├── viewer.cpp │ └── visual_odometry.cpp └── docs/ ├── _sidebar.md ├── chapter1/ │ └── chapter1.md ├── chapter10/ │ └── chapter10.md ├── chapter11/ │ └── chapter11.md ├── chapter12/ │ └── chapter12.md ├── chapter2/ │ └── chapter2.md ├── chapter3/ │ └── chapter3.md ├── chapter4/ │ └── chapter4.md ├── chapter5/ │ └── chapter5.md ├── chapter6/ │ └── chapter6.md ├── chapter7/ │ └── chapter7.md ├── chapter8/ │ └── chapter8.md ├── chapter9/ │ └── chapter9.md └── reference_answers_for_each_chapter/ └── reference_answers_for_each_chapter.md ================================================ FILE CONTENTS ================================================ ================================================ FILE: LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 2, June 1991 Copyright (C) 1989, 1991 Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The licenses for most software are designed to take away your freedom to share and change it. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change free software--to make sure the software is free for all its users. This General Public License applies to most of the Free Software Foundation's software and to any other program whose authors commit to using it. (Some other Free Software Foundation software is covered by the GNU Lesser General Public License instead.) You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for this service if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs; and that you know you can do these things. To protect your rights, we need to make restrictions that forbid anyone to deny you these rights or to ask you to surrender the rights. These restrictions translate to certain responsibilities for you if you distribute copies of the software, or if you modify it. For example, if you distribute copies of such a program, whether gratis or for a fee, you must give the recipients all the rights that you have. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. We protect your rights with two steps: (1) copyright the software, and (2) offer you this license which gives you legal permission to copy, distribute and/or modify the software. Also, for each author's protection and ours, we want to make certain that everyone understands that there is no warranty for this free software. If the software is modified by someone else and passed on, we want its recipients to know that what they have is not the original, so that any problems introduced by others will not reflect on the original authors' reputations. Finally, any free program is threatened constantly by software patents. We wish to avoid the danger that redistributors of a free program will individually obtain patent licenses, in effect making the program proprietary. To prevent this, we have made it clear that any patent must be licensed for everyone's free use or not licensed at all. The precise terms and conditions for copying, distribution and modification follow. GNU GENERAL PUBLIC LICENSE TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 0. This License applies to any program or other work which contains a notice placed by the copyright holder saying it may be distributed under the terms of this General Public License. The "Program", below, refers to any such program or work, and a "work based on the Program" means either the Program or any derivative work under copyright law: that is to say, a work containing the Program or a portion of it, either verbatim or with modifications and/or translated into another language. (Hereinafter, translation is included without limitation in the term "modification".) Each licensee is addressed as "you". Activities other than copying, distribution and modification are not covered by this License; they are outside its scope. The act of running the Program is not restricted, and the output from the Program is covered only if its contents constitute a work based on the Program (independent of having been made by running the Program). Whether that is true depends on what the Program does. 1. You may copy and distribute verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice and disclaimer of warranty; keep intact all the notices that refer to this License and to the absence of any warranty; and give any other recipients of the Program a copy of this License along with the Program. You may charge a fee for the physical act of transferring a copy, and you may at your option offer warranty protection in exchange for a fee. 2. You may modify your copy or copies of the Program or any portion of it, thus forming a work based on the Program, and copy and distribute such modifications or work under the terms of Section 1 above, provided that you also meet all of these conditions: a) You must cause the modified files to carry prominent notices stating that you changed the files and the date of any change. b) You must cause any work that you distribute or publish, that in whole or in part contains or is derived from the Program or any part thereof, to be licensed as a whole at no charge to all third parties under the terms of this License. c) If the modified program normally reads commands interactively when run, you must cause it, when started running for such interactive use in the most ordinary way, to print or display an announcement including an appropriate copyright notice and a notice that there is no warranty (or else, saying that you provide a warranty) and that users may redistribute the program under these conditions, and telling the user how to view a copy of this License. (Exception: if the Program itself is interactive but does not normally print such an announcement, your work based on the Program is not required to print an announcement.) These requirements apply to the modified work as a whole. If identifiable sections of that work are not derived from the Program, and can be reasonably considered independent and separate works in themselves, then this License, and its terms, do not apply to those sections when you distribute them as separate works. But when you distribute the same sections as part of a whole which is a work based on the Program, the distribution of the whole must be on the terms of this License, whose permissions for other licensees extend to the entire whole, and thus to each and every part regardless of who wrote it. Thus, it is not the intent of this section to claim rights or contest your rights to work written entirely by you; rather, the intent is to exercise the right to control the distribution of derivative or collective works based on the Program. In addition, mere aggregation of another work not based on the Program with the Program (or with a work based on the Program) on a volume of a storage or distribution medium does not bring the other work under the scope of this License. 3. You may copy and distribute the Program (or a work based on it, under Section 2) in object code or executable form under the terms of Sections 1 and 2 above provided that you also do one of the following: a) Accompany it with the complete corresponding machine-readable source code, which must be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange; or, b) Accompany it with a written offer, valid for at least three years, to give any third party, for a charge no more than your cost of physically performing source distribution, a complete machine-readable copy of the corresponding source code, to be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange; or, c) Accompany it with the information you received as to the offer to distribute corresponding source code. (This alternative is allowed only for noncommercial distribution and only if you received the program in object code or executable form with such an offer, in accord with Subsection b above.) The source code for a work means the preferred form of the work for making modifications to it. For an executable work, complete source code means all the source code for all modules it contains, plus any associated interface definition files, plus the scripts used to control compilation and installation of the executable. However, as a special exception, the source code distributed need not include anything that is normally distributed (in either source or binary form) with the major components (compiler, kernel, and so on) of the operating system on which the executable runs, unless that component itself accompanies the executable. If distribution of executable or object code is made by offering access to copy from a designated place, then offering equivalent access to copy the source code from the same place counts as distribution of the source code, even though third parties are not compelled to copy the source along with the object code. 4. You may not copy, modify, sublicense, or distribute the Program except as expressly provided under this License. Any attempt otherwise to copy, modify, sublicense or distribute the Program is void, and will automatically terminate your rights under this License. However, parties who have received copies, or rights, from you under this License will not have their licenses terminated so long as such parties remain in full compliance. 5. You are not required to accept this License, since you have not signed it. However, nothing else grants you permission to modify or distribute the Program or its derivative works. These actions are prohibited by law if you do not accept this License. Therefore, by modifying or distributing the Program (or any work based on the Program), you indicate your acceptance of this License to do so, and all its terms and conditions for copying, distributing or modifying the Program or works based on it. 6. Each time you redistribute the Program (or any work based on the Program), the recipient automatically receives a license from the original licensor to copy, distribute or modify the Program subject to these terms and conditions. You may not impose any further restrictions on the recipients' exercise of the rights granted herein. You are not responsible for enforcing compliance by third parties to this License. 7. If, as a consequence of a court judgment or allegation of patent infringement or for any other reason (not limited to patent issues), conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot distribute so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not distribute the Program at all. For example, if a patent license would not permit royalty-free redistribution of the Program by all those who receive copies directly or indirectly through you, then the only way you could satisfy both it and this License would be to refrain entirely from distribution of the Program. If any portion of this section is held invalid or unenforceable under any particular circumstance, the balance of the section is intended to apply and the section as a whole is intended to apply in other circumstances. It is not the purpose of this section to induce you to infringe any patents or other property right claims or to contest validity of any such claims; this section has the sole purpose of protecting the integrity of the free software distribution system, which is implemented by public license practices. Many people have made generous contributions to the wide range of software distributed through that system in reliance on consistent application of that system; it is up to the author/donor to decide if he or she is willing to distribute software through any other system and a licensee cannot impose that choice. This section is intended to make thoroughly clear what is believed to be a consequence of the rest of this License. 8. If the distribution and/or use of the Program is restricted in certain countries either by patents or by copyrighted interfaces, the original copyright holder who places the Program under this License may add an explicit geographical distribution limitation excluding those countries, so that distribution is permitted only in or among countries not thus excluded. In such case, this License incorporates the limitation as if written in the body of this License. 9. The Free Software Foundation may publish revised and/or new versions of the General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies a version number of this License which applies to it and "any later version", you have the option of following the terms and conditions either of that version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of this License, you may choose any version ever published by the Free Software Foundation. 10. If you wish to incorporate parts of the Program into other free programs whose distribution conditions are different, write to the author to ask for permission. For software which is copyrighted by the Free Software Foundation, write to the Free Software Foundation; we sometimes make exceptions for this. Our decision will be guided by the two goals of preserving the free status of all derivatives of our free software and of promoting the sharing and reuse of software generally. NO WARRANTY 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively convey the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. Also add information on how to contact you by electronic and paper mail. If the program is interactive, make it output a short notice like this when it starts in an interactive mode: Gnomovision version 69, Copyright (C) year name of author Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, the commands you use may be called something other than `show w' and `show c'; they could even be mouse-clicks or menu items--whatever suits your program. You should also get your employer (if you work as a programmer) or your school, if any, to sign a "copyright disclaimer" for the program, if necessary. Here is a sample; alter the names: Yoyodyne, Inc., hereby disclaims all copyright interest in the program `Gnomovision' (which makes passes at compilers) written by James Hacker. , 1 April 1989 Ty Coon, President of Vice This General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. ================================================ FILE: README.md ================================================ # smoothly-vslam - **项目简介** 👋 **欢迎来到VSLAM的世界**
SLAM是一个与传感器紧密联系的系统，VSLAM使用的传感器就是摄像头。处理摄像头数据需要理解相机成像的过程，这是一个从现实世界到计算机能处理的数据的映射过程。通过这节，你会知道相机的成像模型及映射过程的几个坐标系。与之相关的是相机的畸变及内参标定。这对应于第一章。
观测数据所处坐标系为传感器坐标系，为局部观测，我们需要将局部观测转换到全局观测上，这就涉及坐标系间转换。将传感器坐标系观测转换到载体坐标系需要通过外参，计算载体坐标系在世界坐标系下坐标需要用到三维空间中刚体运动变换。这两部分分别对应第二章与第三章。
通过上面三部分，基本就可以知道VSLAM的前端工作的一个背景了。可以想象VSLAM系统是在一个载体上搭载着摄像头，在未知环境中不断移动，对环境及自身位姿进行同时估计。那么什么是VSLAM？这块会在第四章进行介绍。
通过第四章，我们知道目前成熟的VSLAM框架主要包含前端，后端，回环检测及建图四个模块，这会在后续章节依次介绍。
前端为视觉里程计，即VO。我们着重介绍目前使用的较多的特征点法VO，也就是间接法。传统特征点法依赖人工设计的视觉特征，这块会在第五章进行介绍。
基于特征点的提取与匹配，我们可以对相机的位姿及特征点的三维空间位姿进行估计，这部分主要分为两个过程，即初始化过程及帧间位移估计。初始化需要确定三维空间点坐标，世界坐标系及尺度，比较复杂，在初始化完成后，就可以通过特征匹配，比较轻松得获得相机帧间位移。这两个过程会在第六章及第七章进行介绍。
介绍完视觉前端，接下来是视觉后端，按照使用不同技术，分为基于滤波的方法与非线性优化的方法。这两部分对应于第八章及第九章。
然后是回环检测模块，这部分会在第十章进行讲解。
VSLAM最后一个重要模块建图对应于第十一章，这也是主教程最后一个章节。
如果学有余力，可以看第十二章实践章节，亲自设计一个VSLAM系统。如果完成这个章节，你会获得很大的成就感，对于后面工程应用会有很大帮助。
最后就是进度，每天看一点就行。学到很多东西的秘诀，就是每次不要看太多。\ 教程博客在线阅读地址一：[smoothly-vslam](https://www.yuque.com/u1507140/vslam-hmh) ### 推荐书籍 VSLAM属于一个交叉系统学科，包含很多方向的内容，如多视图几何，状态估计，优化等等，以下是部分推荐书籍：
1.多视图几何 - 《Multiple View Geometry in Computer Vision 》 - [《计算机视觉中的数学方法》](http://in.ruc.edu.cn/wp-content/uploads/2021/01/Maths-in-3D-computer-vision.pdf) 2.刚体运动 - 李群和李代数: [Quaternion kinematics for the error-state Kalman filter ](http://www.iri.upc.edu/people/jsola/JoanSola/objectes/notes/kinematics.pdf) - 《机器人学中的状态估计》原作者蒂莫西.D.巴富特，译者高翔。 3.VSLAM介绍 - 《视觉SLAM十四讲:从理论到实践》 - [高翔博士的博客](https://www.cnblogs.com/gaoxiang12/p/3695962.html) - - D. Scaramuzza, F. Fraundorfer, "Visual Odometry: Part I - The First 30 Years and Fundamentals IEEE Robotics and Automation Magazine", Volume 18, issue 4, 2011. - - F. Fraundorfer and D. Scaramuzza, "Visual Odometry : Part II: Matching, Robustness, Optimization, and Applications," in IEEE Robotics & Automation Magazine, vol. 19, no. 2, pp. 78-90, June 2012. 4.概率论 - 《概率机器人》 5.优化理论 - [数值最优化（第二版）(美)乔治·劳斯特(Jorge Nocedal)](https://www.math.uci.edu/~qnie/Publications/NumericalOptimization.pdf) - 凸优化：[Convex Optimization ](https://web.stanford.edu/~boyd/cvxbook/bv_cvxbook.pdf) - 凸优化：[Introductory Lectures on Convex Programming](https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.693.855&rep=rep1&type=pdf) - [g2o](http://ais.informatik.uni-freiburg.de/publications/papers/kuemmerle11icra.pdf) - 《机器人感知：因子图在SLAM中的应用》（gtsam作者，佐治亚理工教授 Frank Dellaert写的） 优化理论进阶 - Nolinear Programming 6.一些工具 - ubuntu, cmake, [bash](https://www.zhihu.com/search?q=bash&search_source=Entity&hybrid_search_source=Entity&hybrid_search_extra=%7B%22sourceType%22%3A%22answer%22%2C%22sourceId%22%3A145219653%7D), vim, qt(optional). - OpenCV install, read the opencv reference manual and tutorial. - ros install： [ROS/Installation - ROS Wiki](https://link.zhihu.com/?target=http%3A//wiki.ros.org/ROS/Installation) - ros [tutorial](https://www.zhihu.com/search?q=tutorial&search_source=Entity&hybrid_search_source=Entity&hybrid_search_extra=%7B%22sourceType%22%3A%22answer%22%2C%22sourceId%22%3A145219653%7D): [ROS/Tutorials - ROS Wiki](https://link.zhihu.com/?target=http%3A//wiki.ros.org/ROS/Tutorials) \ 如果你想对本教程做贡献，请邮件联系：530019431@qq.com Finally, hope you enjoy it！ - **项目内容目录** \ **0.前言** \ **1.一幅图像的诞生：相机投影及相机畸变** \ **2.差异与统一：坐标系变换与外参标定** \ **3.描述状态不简单：三维空间刚体运动** \ **4.也见森林：视觉SLAM简介** \ **5.以不变应万变：前端-视觉里程计之特征点** \ **6.换一个视角看世界：前端视觉里程计之对极几何** \ **7.积硅步以至千里：前端-视觉里程计之相对位姿估计** \ **8.在不确定中寻找确定：后端之卡尔曼滤波** \ **9.每次更好，就是最好：后端之非线性优化** \ **10.又回到曾经的地点：回环检测** \ **11.终识庐山真面目：建图** \ **12.实践是检验真理的唯一标准：设计VSLAM系统** - **项目在线阅读地址**\ https://www.yuque.com/u1507140/vslam-hmh ## Roadmap 这里列一些还需要完善的部分 ### 1.对各章节内容的进一步的完善 补充各章节的基础内容，从背景发展到具体的算法原理的进一步充实 比如，对第八章，卡尔曼滤波可以有如下的进一步完善 - 1.贝叶斯滤波的补充 - 2.其他滤波的介绍 ### 2.内容的进阶，不仅限于VSLAM内容 - 1.SLAM更一般的理论基础，如可观性，退化场景分析 - 2.SLAM应用场景等等 **当前教程为VSLAM基础教程，涉及VSLAM背景知识及基础算法原理，对更深入的部分，计划后续开一个进阶教程进行讲解。** ## 参与贡献 - 如果你想参与到项目中来欢迎查看项目的 [Issue]() 查看没有被分配的任务。 - 如果你发现了一些问题，欢迎在 [Issue]() 中进行反馈🐛。 - 如果你对本项目感兴趣想要参与进来可以通过 [Discussion]() 进行交流💬。 如果你对 Datawhale 很感兴趣并想要发起一个新的项目，欢迎查看 [Datawhale 贡献指南](https://github.com/datawhalechina/DOPMC#%E4%B8%BA-datawhale-%E5%81%9A%E5%87%BA%E8%B4%A1%E7%8C%AE)。 ## 贡献者名单 | 姓名 | 职责 | 简介 | 联系方式| | :----| :---- | :---- |:---- | | 胡明豪 | 项目负责人，教程初版贡献者 | DataWhale成员，VSLAM算法工程师 |530019431@qq.com| | 王洲烽 | 第1，2章贡献者 | DataWhale成员，国防科技大学准研究生 | wangzhoufeng7346@gmail.com | | 乔建森 | 第3，5，8，9章贡献者| 中国航天科工三院-算法工程师 | 2719008838@qq.com | | 林俊强 | 第5章贡献者| 算法工程师 | hayasijq@gmail.com | ## 关注我们

扫描下方二维码关注公众号：Datawhale

## LICENSE 知识共享许可协议
本作品采用知识共享署名-非商业性使用-相同方式共享 4.0 国际许可协议进行许可。 ================================================ FILE: TASK.md ================================================ # smoothly-vslam 基本信息 \ 本项目涉及VSLAM基础概念及算法原理，通过本项目内容，降低学习者后续学习开源vslam算法的难度，平滑小白学习vslam的学习曲线，为后续学习者掌握复杂的vslam算法打下基础。 学习周期：20天，每天平均花费时间 2小时-4小时不等，根据个人学习接受能力强弱有所浮动 学习形式：教程学习+实践 人群定位：具备一定编程基础，有学习和梳理VSLAM算法的需求。 先修内容：无 难度系数：中 ## 任务安排 ### Task 1: 相机投影及内外参 （3天） 理论部分 1.1 介绍针孔相机模型 1.2相机成像的几个坐标系 1.3 内参矩阵及去畸变的方法 2.1介绍 坐标系变换 2.2介绍 相机外参标定 教程地址: [1.一幅图像的诞生：相机投影及相机畸变](https://www.yuque.com/u1507140/vslam-hmh/ogc8v31hbzb6efy8) \ [2. 差异与统一：坐标系变换与外参标定](https://www.yuque.com/u1507140/vslam-hmh/csqub9k4nax99i19) ### Task 2: 三维空间刚体运动 （2天） 理论部分 介绍 世界坐标系与里程计坐标系 介绍 三维刚体运动 介绍 旋转的参数化 教程地址 [3.描述状态不简单：三维空间刚体运动](https://www.yuque.com/u1507140/vslam-hmh/pn5az0nwigy51f25) ### Task 3: 视觉SLAM简介 （1天） 理论部分 介绍 什么是视觉SLAM 介绍 视觉SLAM的主流框架及分类 教程地址 [4. 也见森林：视觉SLAM简介](https://www.yuque.com/u1507140/vslam-hmh/xkqtacgm3gg6crk5) ### Task 4: 前端-视觉里程计之特征点 （3天） 理论部分 介绍 图像特征简述 介绍 常用特征点 教程地址 [5.以不变应万变：前端-视觉里程计之特征点](https://www.yuque.com/u1507140/vslam-hmh/rcvyw38lhgchkb6g) ### Task 5: 前端-对极几何及相对位姿估计 （3天） 理论部分 1.1介绍 对极几何 1.2介绍 本质矩阵及其求解 1.3介绍 单应矩阵及其求解 1.4介绍 三角测量 2.1介绍 已知3D-2D匹配关系，估计相机相对位姿的常用算法 教程地址 [6.换一个视角看世界：前端-视觉里程计之对极几何](https://www.yuque.com/u1507140/vslam-hmh/yu9032oczfnbnr2y) \ [7.积硅步以至千里：前端-视觉里程计之相对位姿估计](https://www.yuque.com/u1507140/vslam-hmh/lusyrekv9r5g0q4n) ### Task 6: 后端之卡尔曼滤波与非线性优化 （4天） 理论部分 1.1 介绍 卡尔曼滤波原理 2.1介绍 最小二乘问题 2.2介绍 牛顿法 2.3介绍 高斯牛顿法 2.4介绍 LM法 教程地址 [8.在不确定中寻找确定：后端之卡尔曼滤波](https://www.yuque.com/u1507140/vslam-hmh/fsblfmf5te9egulq) \ [9.每次更好，就是最好：后端之非线性优化](https://www.yuque.com/u1507140/vslam-hmh/swh0g4a0t452pwux) ### Task 7: 回环检测 （2天） 理论部分 介绍 回环检测的作用 介绍 视觉常用回环检测方法 介绍 使用词袋模型进行回环检测 教程地址 [10.又回到曾经的地点：回环检测](https://www.yuque.com/u1507140/vslam-hmh/mrx3a5aotqr412qe) ### Task 8: 建图 （2天） 理论部分 介绍 地图的类型 介绍 稠密重建 介绍 点云地图 介绍 TSDF地图 介绍 实时三维重建 教程地址 [11.终识庐山真面目：建图](https://www.yuque.com/u1507140/vslam-hmh/ow0woydmogl8wc6n) ### Task 9: 设计VSLAM系统 （2天）选做 理论部分 介绍 C++的工程结构 介绍 系统设计方法 练习 设计一个VSLAM系统 教程地址 [*12.实践是检验真理的唯一标准：设计VSLAM系统](https://www.yuque.com/u1507140/vslam-hmh/cgl78erifda00hge) ================================================ FILE: code/ch10/CMakeLists.txt ================================================ cmake_minimum_required( VERSION 2.8 ) project( loop_closure ) set( CMAKE_BUILD_TYPE "Release" ) set( CMAKE_CXX_FLAGS "-std=c++11 -O3" ) # opencv find_package( OpenCV 3.1 REQUIRED ) include_directories( ${OpenCV_INCLUDE_DIRS} ) # dbow3 # dbow3 is a simple lib so I assume you installed it in default directory set( DBoW3_INCLUDE_DIRS "/usr/local/include" ) #set( DBoW3_LIBS "/usr/local/lib/libDBoW3.a" ) set( DBoW3_LIBS "/usr/local/lib/libDBoW3.so" ) add_executable( feature_training feature_training.cpp ) target_link_libraries( feature_training ${OpenCV_LIBS} ${DBoW3_LIBS} ) add_executable( loop_closure loop_closure.cpp ) target_link_libraries( loop_closure ${OpenCV_LIBS} ${DBoW3_LIBS} ) add_executable( gen_vocab gen_vocab_large.cpp ) target_link_libraries( gen_vocab ${OpenCV_LIBS} ${DBoW3_LIBS} ) ================================================ FILE: code/ch10/cmake_modules/FindDBoW3.cmake ================================================ ================================================ FILE: code/ch10/feature_training.cpp ================================================ #include "DBoW3/DBoW3.h" #include #include #include #include #include #include using namespace cv; using namespace std; /*************************************************** * 本节演示了如何根据data/目录下的十张图训练字典 * ************************************************/ int main( int argc, char** argv ) { // read the image cout<<"reading images... "< images; for ( int i=0; i<10; i++ ) { string path = "../data/"+to_string(i+1)+".png"; images.push_back( imread(path) ); } // detect ORB features cout<<"detecting ORB features ... "< detector = ORB::create(); vector descriptors; for ( Mat& image:images ) { vector keypoints; Mat descriptor; detector->detectAndCompute( image, Mat(), keypoints, descriptor ); descriptors.push_back( descriptor ); } // create vocabulary cout<<"creating vocabulary ... "< #include #include #include #include #include using namespace cv; using namespace std; int main( int argc, char** argv ) { string dataset_dir = argv[1]; ifstream fin ( dataset_dir+"/associate.txt" ); if ( !fin ) { cout<<"please generate the associate file called associate.txt!"< rgb_files, depth_files; vector rgb_times, depth_times; while ( !fin.eof() ) { string rgb_time, rgb_file, depth_time, depth_file; fin>>rgb_time>>rgb_file>>depth_time>>depth_file; rgb_times.push_back ( atof ( rgb_time.c_str() ) ); depth_times.push_back ( atof ( depth_time.c_str() ) ); rgb_files.push_back ( dataset_dir+"/"+rgb_file ); depth_files.push_back ( dataset_dir+"/"+depth_file ); if ( fin.good() == false ) break; } fin.close(); cout<<"generating features ... "< descriptors; Ptr< Feature2D > detector = ORB::create(); int index = 1; for ( string rgb_file:rgb_files ) { Mat image = imread(rgb_file); vector keypoints; Mat descriptor; detector->detectAndCompute( image, Mat(), keypoints, descriptor ); descriptors.push_back( descriptor ); cout<<"extracting features from image " << index++ < #include #include #include #include #include using namespace cv; using namespace std; /*************************************************** * 本节演示了如何根据前面训练的字典计算相似性评分 * ************************************************/ int main(int argc, char **argv) { // read the images and database cout << "reading database" << endl; DBoW3::Vocabulary vocab("./vocabulary.yml.gz"); // DBoW3::Vocabulary vocab("./vocab_larger.yml.gz"); // use large vocab if you want: if (vocab.empty()) { cerr << "Vocabulary does not exist." << endl; return 1; } cout << "reading images... " << endl; vector images; for (int i = 0; i < 10; i++) { string path = "../data/" + to_string(i + 1) + ".png"; images.push_back(imread(path)); } // NOTE: in this case we are comparing images with a vocabulary generated by themselves, this may lead to overfit. // detect ORB features cout << "detecting ORB features ... " << endl; Ptr detector = ORB::create(); vector descriptors; for (Mat &image:images) { vector keypoints; Mat descriptor; detector->detectAndCompute(image, Mat(), keypoints, descriptor); descriptors.push_back(descriptor); } // we can compare the images directly or we can compare one image to a database // images : cout << "comparing images with images " << endl; for (int i = 0; i < images.size(); i++) { DBoW3::BowVector v1; vocab.transform(descriptors[i], v1); for (int j = i; j < images.size(); j++) { DBoW3::BowVector v2; vocab.transform(descriptors[j], v2); double score = vocab.score(v1, v2); cout << "image " << i << " vs image " << j << " : " << score << endl; } cout << endl; } // or with database cout << "comparing images with database " << endl; DBoW3::Database db(vocab, false, 0); for (int i = 0; i < descriptors.size(); i++) db.add(descriptors[i]); cout << "database info: " << db << endl; for (int i = 0; i < descriptors.size(); i++) { DBoW3::QueryResults ret; db.query(descriptors[i], ret, 4); // max result=4 cout << "searching for image " << i << " returns " << ret << endl << endl; } cout << "done." << endl; } ================================================ FILE: code/ch11/dense_mono/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8) project(dense_monocular) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11 -march=native -O3") ############### dependencies ###################### # Eigen find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIR}) include_directories(${FMT_INCLUDE_DIRS}) find_package(FMT REQUIRED) # OpenCV find_package(OpenCV 3.1 REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) # Sophus find_package(Sophus REQUIRED) include_directories(${Sophus_INCLUDE_DIRS}) set(THIRD_PARTY_LIBS ${OpenCV_LIBS} ${Sophus_LIBRARIES}) add_executable(dense_mapping dense_mapping.cpp) target_link_libraries(dense_mapping ${THIRD_PARTY_LIBS} fmt::fmt) ================================================ FILE: code/ch11/dense_mono/dense_mapping.cpp ================================================ #include #include #include using namespace std; #include // for sophus #include using Sophus::SE3d; // for eigen #include #include using namespace Eigen; #include #include #include using namespace cv; /********************************************** * 本程序演示了单目相机在已知轨迹下的稠密深度估计 * 使用极线搜索 + NCC 匹配的方式，与书本的 12.2 节对应 * 请注意本程序并不完美，你完全可以改进它——我其实在故意暴露一些问题(这是借口)。 ***********************************************/ // ------------------------------------------------------------------ // parameters const int boarder = 20; // 边缘宽度 const int width = 640; // 图像宽度 const int height = 480; // 图像高度 const double fx = 481.2f; // 相机内参 const double fy = -480.0f; const double cx = 319.5f; const double cy = 239.5f; const int ncc_window_size = 3; // NCC 取的窗口半宽度 const int ncc_area = (2 * ncc_window_size + 1) * (2 * ncc_window_size + 1); // NCC窗口面积 const double min_cov = 0.1; // 收敛判定：最小方差 const double max_cov = 10; // 发散判定：最大方差 // ------------------------------------------------------------------ // 重要的函数 /// 从 REMODE 数据集读取数据 bool readDatasetFiles( const string &path, vector &color_image_files, vector &poses, cv::Mat &ref_depth ); /** * 根据新的图像更新深度估计 * @param ref 参考图像 * @param curr 当前图像 * @param T_C_R 参考图像到当前图像的位姿 * @param depth 深度 * @param depth_cov 深度方差 * @return 是否成功 */ bool update( const Mat &ref, const Mat &curr, const SE3d &T_C_R, Mat &depth, Mat &depth_cov2 ); /** * 极线搜索 * @param ref 参考图像 * @param curr 当前图像 * @param T_C_R 位姿 * @param pt_ref 参考图像中点的位置 * @param depth_mu 深度均值 * @param depth_cov 深度方差 * @param pt_curr 当前点 * @param epipolar_direction 极线方向 * @return 是否成功 */ bool epipolarSearch( const Mat &ref, const Mat &curr, const SE3d &T_C_R, const Vector2d &pt_ref, const double &depth_mu, const double &depth_cov, Vector2d &pt_curr, Vector2d &epipolar_direction ); /** * 更新深度滤波器 * @param pt_ref 参考图像点 * @param pt_curr 当前图像点 * @param T_C_R 位姿 * @param epipolar_direction 极线方向 * @param depth 深度均值 * @param depth_cov2 深度方向 * @return 是否成功 */ bool updateDepthFilter( const Vector2d &pt_ref, const Vector2d &pt_curr, const SE3d &T_C_R, const Vector2d &epipolar_direction, Mat &depth, Mat &depth_cov2 ); /** * 计算 NCC 评分 * @param ref 参考图像 * @param curr 当前图像 * @param pt_ref 参考点 * @param pt_curr 当前点 * @return NCC评分 */ double NCC(const Mat &ref, const Mat &curr, const Vector2d &pt_ref, const Vector2d &pt_curr); // 双线性灰度插值 inline double getBilinearInterpolatedValue(const Mat &img, const Vector2d &pt) { uchar *d = &img.data[int(pt(1, 0)) * img.step + int(pt(0, 0))]; double xx = pt(0, 0) - floor(pt(0, 0)); double yy = pt(1, 0) - floor(pt(1, 0)); return ((1 - xx) * (1 - yy) * double(d[0]) + xx * (1 - yy) * double(d[1]) + (1 - xx) * yy * double(d[img.step]) + xx * yy * double(d[img.step + 1])) / 255.0; } // ------------------------------------------------------------------ // 一些小工具 // 显示估计的深度图 void plotDepth(const Mat &depth_truth, const Mat &depth_estimate); // 像素到相机坐标系 inline Vector3d px2cam(const Vector2d px) { return Vector3d( (px(0, 0) - cx) / fx, (px(1, 0) - cy) / fy, 1 ); } // 相机坐标系到像素 inline Vector2d cam2px(const Vector3d p_cam) { return Vector2d( p_cam(0, 0) * fx / p_cam(2, 0) + cx, p_cam(1, 0) * fy / p_cam(2, 0) + cy ); } // 检测一个点是否在图像边框内 inline bool inside(const Vector2d &pt) { return pt(0, 0) >= boarder && pt(1, 0) >= boarder && pt(0, 0) + boarder < width && pt(1, 0) + boarder <= height; } // 显示极线匹配 void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr); // 显示极线 void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr, const Vector2d &px_max_curr); /// 评测深度估计 void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate); // ------------------------------------------------------------------ int main(int argc, char **argv) { if (argc != 2) { cout << "Usage: dense_mapping path_to_test_dataset" << endl; return -1; } // 从数据集读取数据 vector color_image_files; vector poses_TWC; Mat ref_depth; bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC, ref_depth); if (ret == false) { cout << "Reading image files failed!" << endl; return -1; } cout << "read total " << color_image_files.size() << " files." << endl; // 第一张图 Mat ref = imread(color_image_files[0], 0); // gray-scale image SE3d pose_ref_TWC = poses_TWC[0]; double init_depth = 3.0; // 深度初始值 double init_cov2 = 3.0; // 方差初始值 Mat depth(height, width, CV_64F, init_depth); // 深度图 Mat depth_cov2(height, width, CV_64F, init_cov2); // 深度图方差 for (int index = 1; index < color_image_files.size(); index++) { cout << "*** loop " << index << " ***" << endl; Mat curr = imread(color_image_files[index], 0); if (curr.data == nullptr) continue; SE3d pose_curr_TWC = poses_TWC[index]; SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC; // 坐标转换关系： T_C_W * T_W_R = T_C_R update(ref, curr, pose_T_C_R, depth, depth_cov2); evaludateDepth(ref_depth, depth); plotDepth(ref_depth, depth); // imshow("image", curr); waitKey(); } cout << "estimation returns, saving depth map ..." << endl; imwrite("depth.png", depth); cout << "done." << endl; return 0; } bool readDatasetFiles( const string &path, vector &color_image_files, std::vector &poses, cv::Mat &ref_depth) { ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt"); if (!fin) return false; while (!fin.eof()) { // 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW string image; fin >> image; double data[7]; for (double &d:data) fin >> d; color_image_files.push_back(path + string("/images/") + image); poses.push_back( SE3d(Quaterniond(data[6], data[3], data[4], data[5]), Vector3d(data[0], data[1], data[2])) ); if (!fin.good()) break; } fin.close(); // load reference depth fin.open(path + "/depthmaps/scene_000.depth"); ref_depth = cv::Mat(height, width, CV_64F); if (!fin) return false; for (int y = 0; y < height; y++) for (int x = 0; x < width; x++) { double depth = 0; fin >> depth; ref_depth.ptr(y)[x] = depth / 100.0; } return true; } // 对整个深度图进行更新 bool update(const Mat &ref, const Mat &curr, const SE3d &T_C_R, Mat &depth, Mat &depth_cov2) { for (int x = boarder; x < width - boarder; x++) for (int y = boarder; y < height - boarder; y++) { // 遍历每个像素 if (depth_cov2.ptr(y)[x] < min_cov || depth_cov2.ptr(y)[x] > max_cov) // 深度已收敛或发散 continue; // 在极线上搜索 (x,y) 的匹配 Vector2d pt_curr; Vector2d epipolar_direction; bool ret = epipolarSearch( ref, curr, T_C_R, Vector2d(x, y), depth.ptr(y)[x], sqrt(depth_cov2.ptr(y)[x]), pt_curr, epipolar_direction ); if (ret == false) // 匹配失败 continue; // 取消该注释以显示匹配 // showEpipolarMatch(ref, curr, Vector2d(x, y), pt_curr); // 匹配成功，更新深度图 updateDepthFilter(Vector2d(x, y), pt_curr, T_C_R, epipolar_direction, depth, depth_cov2); } } // 极线搜索 // 方法见书 12.2 12.3 两节 bool epipolarSearch( const Mat &ref, const Mat &curr, const SE3d &T_C_R, const Vector2d &pt_ref, const double &depth_mu, const double &depth_cov, Vector2d &pt_curr, Vector2d &epipolar_direction) { Vector3d f_ref = px2cam(pt_ref); f_ref.normalize(); Vector3d P_ref = f_ref * depth_mu; // 参考帧的 P 向量 Vector2d px_mean_curr = cam2px(T_C_R * P_ref); // 按深度均值投影的像素 double d_min = depth_mu - 3 * depth_cov, d_max = depth_mu + 3 * depth_cov; if (d_min < 0.1) d_min = 0.1; Vector2d px_min_curr = cam2px(T_C_R * (f_ref * d_min)); // 按最小深度投影的像素 Vector2d px_max_curr = cam2px(T_C_R * (f_ref * d_max)); // 按最大深度投影的像素 Vector2d epipolar_line = px_max_curr - px_min_curr; // 极线（线段形式） epipolar_direction = epipolar_line; // 极线方向 epipolar_direction.normalize(); double half_length = 0.5 * epipolar_line.norm(); // 极线线段的半长度 if (half_length > 100) half_length = 100; // 我们不希望搜索太多东西 // 取消此句注释以显示极线（线段） // showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr ); // 在极线上搜索，以深度均值点为中心，左右各取半长度 double best_ncc = -1.0; Vector2d best_px_curr; for (double l = -half_length; l <= half_length; l += 0.7) { // l+=sqrt(2) Vector2d px_curr = px_mean_curr + l * epipolar_direction; // 待匹配点 if (!inside(px_curr)) continue; // 计算待匹配点与参考帧的 NCC double ncc = NCC(ref, curr, pt_ref, px_curr); if (ncc > best_ncc) { best_ncc = ncc; best_px_curr = px_curr; } } if (best_ncc < 0.85f) // 只相信 NCC 很高的匹配 return false; pt_curr = best_px_curr; return true; } double NCC( const Mat &ref, const Mat &curr, const Vector2d &pt_ref, const Vector2d &pt_curr) { // 零均值-归一化互相关 // 先算均值 double mean_ref = 0, mean_curr = 0; vector values_ref, values_curr; // 参考帧和当前帧的均值 for (int x = -ncc_window_size; x <= ncc_window_size; x++) for (int y = -ncc_window_size; y <= ncc_window_size; y++) { double value_ref = double(ref.ptr(int(y + pt_ref(1, 0)))[int(x + pt_ref(0, 0))]) / 255.0; mean_ref += value_ref; double value_curr = getBilinearInterpolatedValue(curr, pt_curr + Vector2d(x, y)); mean_curr += value_curr; values_ref.push_back(value_ref); values_curr.push_back(value_curr); } mean_ref /= ncc_area; mean_curr /= ncc_area; // 计算 Zero mean NCC double numerator = 0, demoniator1 = 0, demoniator2 = 0; for (int i = 0; i < values_ref.size(); i++) { double n = (values_ref[i] - mean_ref) * (values_curr[i] - mean_curr); numerator += n; demoniator1 += (values_ref[i] - mean_ref) * (values_ref[i] - mean_ref); demoniator2 += (values_curr[i] - mean_curr) * (values_curr[i] - mean_curr); } return numerator / sqrt(demoniator1 * demoniator2 + 1e-10); // 防止分母出现零 } bool updateDepthFilter( const Vector2d &pt_ref, const Vector2d &pt_curr, const SE3d &T_C_R, const Vector2d &epipolar_direction, Mat &depth, Mat &depth_cov2) { // 不知道这段还有没有人看 // 用三角化计算深度 SE3d T_R_C = T_C_R.inverse(); Vector3d f_ref = px2cam(pt_ref); f_ref.normalize(); Vector3d f_curr = px2cam(pt_curr); f_curr.normalize(); // 方程 // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC // f2 = R_RC * f_cur // 转化成下面这个矩阵方程组 // => [ f_ref^T f_ref, -f_ref^T f2 ] [d_ref] [f_ref^T t] // [ f_2^T f_ref, -f2^T f2 ] [d_cur] = [f2^T t ] Vector3d t = T_R_C.translation(); Vector3d f2 = T_R_C.so3() * f_curr; Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2)); Matrix2d A; A(0, 0) = f_ref.dot(f_ref); A(0, 1) = -f_ref.dot(f2); A(1, 0) = -A(0, 1); A(1, 1) = -f2.dot(f2); Vector2d ans = A.inverse() * b; Vector3d xm = ans[0] * f_ref; // ref 侧的结果 Vector3d xn = t + ans[1] * f2; // cur 结果 Vector3d p_esti = (xm + xn) / 2.0; // P的位置，取两者的平均 double depth_estimation = p_esti.norm(); // 深度值 // 计算不确定性（以一个像素为误差） Vector3d p = f_ref * depth_estimation; Vector3d a = p - t; double t_norm = t.norm(); double a_norm = a.norm(); double alpha = acos(f_ref.dot(t) / t_norm); double beta = acos(-a.dot(t) / (a_norm * t_norm)); Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction); f_curr_prime.normalize(); double beta_prime = acos(f_curr_prime.dot(-t) / t_norm); double gamma = M_PI - alpha - beta_prime; double p_prime = t_norm * sin(beta_prime) / sin(gamma); double d_cov = p_prime - depth_estimation; double d_cov2 = d_cov * d_cov; // 高斯融合 double mu = depth.ptr(int(pt_ref(1, 0)))[int(pt_ref(0, 0))]; double sigma2 = depth_cov2.ptr(int(pt_ref(1, 0)))[int(pt_ref(0, 0))]; double mu_fuse = (d_cov2 * mu + sigma2 * depth_estimation) / (sigma2 + d_cov2); double sigma_fuse2 = (sigma2 * d_cov2) / (sigma2 + d_cov2); depth.ptr(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = mu_fuse; depth_cov2.ptr(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2; return true; } // 后面这些太简单我就不注释了（其实是因为懒） void plotDepth(const Mat &depth_truth, const Mat &depth_estimate) { imshow("depth_truth", depth_truth * 0.4); imshow("depth_estimate", depth_estimate * 0.4); imshow("depth_error", depth_truth - depth_estimate); // waitKey(1); } void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate) { double ave_depth_error = 0; // 平均误差 double ave_depth_error_sq = 0; // 平方误差 int cnt_depth_data = 0; for (int y = boarder; y < depth_truth.rows - boarder; y++) for (int x = boarder; x < depth_truth.cols - boarder; x++) { double error = depth_truth.ptr(y)[x] - depth_estimate.ptr(y)[x]; ave_depth_error += error; ave_depth_error_sq += error * error; cnt_depth_data++; } ave_depth_error /= cnt_depth_data; ave_depth_error_sq /= cnt_depth_data; cout << "Average squared error = " << ave_depth_error_sq << ", average error: " << ave_depth_error << endl; } void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr) { Mat ref_show, curr_show; cv::cvtColor(ref, ref_show, CV_GRAY2BGR); cv::cvtColor(curr, curr_show, CV_GRAY2BGR); cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 0, 250), 2); cv::circle(curr_show, cv::Point2f(px_curr(0, 0), px_curr(1, 0)), 5, cv::Scalar(0, 0, 250), 2); imshow("ref", ref_show); imshow("curr", curr_show); waitKey(1); } void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr, const Vector2d &px_max_curr) { Mat ref_show, curr_show; cv::cvtColor(ref, ref_show, CV_GRAY2BGR); cv::cvtColor(curr, curr_show, CV_GRAY2BGR); cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 255, 0), 2); cv::circle(curr_show, cv::Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2); cv::circle(curr_show, cv::Point2f(px_max_curr(0, 0), px_max_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2); cv::line(curr_show, Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), Point2f(px_max_curr(0, 0), px_max_curr(1, 0)), Scalar(0, 255, 0), 1); imshow("ref", ref_show); imshow("curr", curr_show); waitKey(1); } ================================================ FILE: code/ch12/myslam/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8) project(myslam) set(CMAKE_BUILD_TYPE Release) set(CMAKE_CXX_FLAGS "-std=c++11 -Wall") set(CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -O3 -fopenmp -pthread") list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) ############### dependencies ###################### # Eigen include_directories("/usr/include/eigen3") # OpenCV find_package(OpenCV 3.1 REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) # pangolin find_package(Pangolin REQUIRED) include_directories(${Pangolin_INCLUDE_DIRS}) # Sophus find_package(Sophus REQUIRED) include_directories(${Sophus_INCLUDE_DIRS}) # G2O find_package(G2O REQUIRED) include_directories(${G2O_INCLUDE_DIRS}) # glog find_package(Glog REQUIRED) include_directories(${GLOG_INCLUDE_DIRS}) # gflags find_package(GFlags REQUIRED) include_directories(${GFLAGS_INCLUDE_DIRS}) # csparse find_package(CSparse REQUIRED) include_directories(${CSPARSE_INCLUDE_DIR}) set(THIRD_PARTY_LIBS ${OpenCV_LIBS} ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES} GL GLU GLEW glut g2o_core g2o_stuff g2o_types_sba g2o_solver_csparse g2o_csparse_extension ${GTEST_BOTH_LIBRARIES} ${GLOG_LIBRARIES} ${GFLAGS_LIBRARIES} pthread ${CSPARSE_LIBRARY} ) enable_testing() ############### source and test ###################### include_directories(${PROJECT_SOURCE_DIR}/include) add_subdirectory(src) add_subdirectory(app) ================================================ FILE: code/ch12/myslam/app/CMakeLists.txt ================================================ add_executable(run_kitti_stereo run_kitti_stereo.cpp) target_link_libraries(run_kitti_stereo myslam ${THIRD_PARTY_LIBS}) ================================================ FILE: code/ch12/myslam/app/run_kitti_stereo.cpp ================================================ // // Created by gaoxiang on 19-5-4. // #include #include "myslam/visual_odometry.h" DEFINE_string(config_file, "./config/default.yaml", "config file path"); int main(int argc, char **argv) { google::ParseCommandLineFlags(&argc, &argv, true); myslam::VisualOdometry::Ptr vo( new myslam::VisualOdometry(FLAGS_config_file)); assert(vo->Init() == true); vo->Run(); return 0; } ================================================ FILE: code/ch12/myslam/cmake_modules/FindCSparse.cmake ================================================ # Look for csparse; note the difference in the directory specifications! find_path(CSPARSE_INCLUDE_DIR NAMES cs.h PATHS /usr/include/suitesparse /usr/include /opt/local/include /usr/local/include /sw/include /usr/include/ufsparse /opt/local/include/ufsparse /usr/local/include/ufsparse /sw/include/ufsparse PATH_SUFFIXES suitesparse ) find_library(CSPARSE_LIBRARY NAMES cxsparse libcxsparse PATHS /usr/lib /usr/local/lib /opt/local/lib /sw/lib ) include(FindPackageHandleStandardArgs) find_package_handle_standard_args(CSPARSE DEFAULT_MSG CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) ================================================ FILE: code/ch12/myslam/cmake_modules/FindG2O.cmake ================================================ # Find the header files FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h ${G2O_ROOT}/include $ENV{G2O_ROOT}/include $ENV{G2O_ROOT} /usr/local/include /usr/include /opt/local/include /sw/local/include /sw/include NO_DEFAULT_PATH ) # Macro to unify finding both the debug and release versions of the # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY # macro. MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) FIND_LIBRARY("${MYLIBRARY}_DEBUG" NAMES "g2o_${MYLIBRARYNAME}_d" PATHS ${G2O_ROOT}/lib/Debug ${G2O_ROOT}/lib $ENV{G2O_ROOT}/lib/Debug $ENV{G2O_ROOT}/lib NO_DEFAULT_PATH ) FIND_LIBRARY("${MYLIBRARY}_DEBUG" NAMES "g2o_${MYLIBRARYNAME}_d" PATHS ~/Library/Frameworks /Library/Frameworks /usr/local/lib /usr/local/lib64 /usr/lib /usr/lib64 /opt/local/lib /sw/local/lib /sw/lib ) FIND_LIBRARY(${MYLIBRARY} NAMES "g2o_${MYLIBRARYNAME}" PATHS ${G2O_ROOT}/lib/Release ${G2O_ROOT}/lib $ENV{G2O_ROOT}/lib/Release $ENV{G2O_ROOT}/lib NO_DEFAULT_PATH ) FIND_LIBRARY(${MYLIBRARY} NAMES "g2o_${MYLIBRARYNAME}" PATHS ~/Library/Frameworks /Library/Frameworks /usr/local/lib /usr/local/lib64 /usr/lib /usr/lib64 /opt/local/lib /sw/local/lib /sw/lib ) IF(NOT ${MYLIBRARY}_DEBUG) IF(MYLIBRARY) SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) ENDIF(MYLIBRARY) ENDIF( NOT ${MYLIBRARY}_DEBUG) ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) # Find the core elements FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) # Find the CLI library FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) # Find the pluggable solvers FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) # Find the predefined types FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) # G2O solvers declared found if we found at least one solver SET(G2O_SOLVERS_FOUND "NO") IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) SET(G2O_SOLVERS_FOUND "YES") ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) # G2O itself declared found if we found the core libraries and at least one solver SET(G2O_FOUND "NO") IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) SET(G2O_FOUND "YES") ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) ================================================ FILE: code/ch12/myslam/cmake_modules/FindGlog.cmake ================================================ # Ceres Solver - A fast non-linear least squares minimizer # Copyright 2015 Google Inc. All rights reserved. # http://ceres-solver.org/ # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright notice, # this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # * Neither the name of Google Inc. nor the names of its contributors may be # used to endorse or promote products derived from this software without # specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: alexs.mac@gmail.com (Alex Stewart) # # FindGlog.cmake - Find Google glog logging library. # # This module defines the following variables: # # GLOG_FOUND: TRUE iff glog is found. # GLOG_INCLUDE_DIRS: Include directories for glog. # GLOG_LIBRARIES: Libraries required to link glog. # # The following variables control the behaviour of this module: # # GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to # search for glog includes, e.g: /timbuktu/include. # GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to # search for glog libraries, e.g: /timbuktu/lib. # # The following variables are also defined by this module, but in line with # CMake recommended FindPackage() module style should NOT be referenced directly # by callers (use the plural variables detailed above instead). These variables # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which # are NOT re-called (i.e. search for library is not repeated) if these variables # are set with valid values _in the CMake cache_. This means that if these # variables are set directly in the cache, either by the user in the CMake GUI, # or by the user passing -DVAR=VALUE directives to CMake when called (which # explicitly defines a cache variable), then they will be used verbatim, # bypassing the HINTS variables and other hard-coded search locations. # # GLOG_INCLUDE_DIR: Include directory for glog, not including the # include directory of any dependencies. # GLOG_LIBRARY: glog library, not including the libraries of any # dependencies. # Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when # FindGlog was invoked. macro(GLOG_RESET_FIND_LIBRARY_PREFIX) if (MSVC) set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") endif (MSVC) endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) # Called if we failed to find glog or any of it's required dependencies, # unsets all public (designed to be used externally) variables and reports # error message at priority depending upon [REQUIRED/QUIET/] argument. macro(GLOG_REPORT_NOT_FOUND REASON_MSG) unset(GLOG_FOUND) unset(GLOG_INCLUDE_DIRS) unset(GLOG_LIBRARIES) # Make results of search visible in the CMake GUI if glog has not # been found so that user does not have to toggle to advanced view. mark_as_advanced(CLEAR GLOG_INCLUDE_DIR GLOG_LIBRARY) glog_reset_find_library_prefix() # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() # use the camelcase library name, not uppercase. if (Glog_FIND_QUIETLY) message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) elseif (Glog_FIND_REQUIRED) message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) else () # Neither QUIETLY nor REQUIRED, use no priority which emits a message # but continues configuration and allows generation. message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) endif () endmacro(GLOG_REPORT_NOT_FOUND) # Handle possible presence of lib prefix for libraries on MSVC, see # also GLOG_RESET_FIND_LIBRARY_PREFIX(). if (MSVC) # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES # s/t we can set it back before returning. set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") # The empty string in this list is important, it represents the case when # the libraries have no prefix (shared libraries / DLLs). set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") endif (MSVC) # Search user-installed locations first, so that we prefer user installs # to system installs where both exist. list(APPEND GLOG_CHECK_INCLUDE_DIRS /usr/local/include /usr/local/homebrew/include # Mac OS X /opt/local/var/macports/software # Mac OS X. /opt/local/include /usr/include) # Windows (for C:/Program Files prefix). list(APPEND GLOG_CHECK_PATH_SUFFIXES glog/include glog/Include Glog/include Glog/Include) list(APPEND GLOG_CHECK_LIBRARY_DIRS /usr/local/lib /usr/local/homebrew/lib # Mac OS X. /opt/local/lib /usr/lib) # Windows (for C:/Program Files prefix). list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES glog/lib glog/Lib Glog/lib Glog/Lib) # Search supplied hint directories first if supplied. find_path(GLOG_INCLUDE_DIR NAMES glog/logging.h PATHS ${GLOG_INCLUDE_DIR_HINTS} ${GLOG_CHECK_INCLUDE_DIRS} PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) if (NOT GLOG_INCLUDE_DIR OR NOT EXISTS ${GLOG_INCLUDE_DIR}) glog_report_not_found( "Could not find glog include directory, set GLOG_INCLUDE_DIR " "to directory containing glog/logging.h") endif (NOT GLOG_INCLUDE_DIR OR NOT EXISTS ${GLOG_INCLUDE_DIR}) find_library(GLOG_LIBRARY NAMES glog PATHS ${GLOG_LIBRARY_DIR_HINTS} ${GLOG_CHECK_LIBRARY_DIRS} PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) if (NOT GLOG_LIBRARY OR NOT EXISTS ${GLOG_LIBRARY}) glog_report_not_found( "Could not find glog library, set GLOG_LIBRARY " "to full path to libglog.") endif (NOT GLOG_LIBRARY OR NOT EXISTS ${GLOG_LIBRARY}) # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets # if called. set(GLOG_FOUND TRUE) # Glog does not seem to provide any record of the version in its # source tree, thus cannot extract version. # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and # thus FIND_[PATH/LIBRARY] are not called, but specified locations are # invalid, otherwise we would report the library as found. if (GLOG_INCLUDE_DIR AND NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) glog_report_not_found( "Caller defined GLOG_INCLUDE_DIR:" " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") endif (GLOG_INCLUDE_DIR AND NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) # TODO: This regex for glog library is pretty primitive, we use lowercase # for comparison to handle Windows using CamelCase library names, could # this check be better? string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) if (GLOG_LIBRARY AND NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") glog_report_not_found( "Caller defined GLOG_LIBRARY: " "${GLOG_LIBRARY} does not match glog.") endif (GLOG_LIBRARY AND NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") # Set standard CMake FindPackage variables if found. if (GLOG_FOUND) set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) set(GLOG_LIBRARIES ${GLOG_LIBRARY}) endif (GLOG_FOUND) glog_reset_find_library_prefix() # Handle REQUIRED / QUIET optional arguments. include(FindPackageHandleStandardArgs) find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIRS GLOG_LIBRARIES) # Only mark internal variables as advanced if we found glog, otherwise # leave them visible in the standard GUI for the user to set manually. if (GLOG_FOUND) mark_as_advanced(FORCE GLOG_INCLUDE_DIR GLOG_LIBRARY) endif (GLOG_FOUND) ================================================ FILE: code/ch12/myslam/config/default.yaml ================================================ %YAML:1.0 # data # the tum dataset directory, change it to yours! # dataset_dir: /media/xiang/Data/Dataset/Kitti/dataset/sequences/00 dataset_dir: /home/huminghao/hmh/KITTI/00 # camera intrinsics camera.fx: 517.3 camera.fy: 516.5 camera.cx: 325.1 camera.cy: 249.7 num_features: 150 num_features_init: 50 num_features_tracking: 50 ================================================ FILE: code/ch12/myslam/include/myslam/algorithm.h ================================================ // // Created by gaoxiang on 19-5-4. // #ifndef MYSLAM_ALGORITHM_H #define MYSLAM_ALGORITHM_H // algorithms used in myslam #include "myslam/common_include.h" namespace myslam { /** * linear triangulation with SVD * @param poses poses, * @param points points in normalized plane * @param pt_world triangulated point in the world * @return true if success */ inline bool triangulation(const std::vector &poses, const std::vector points, Vec3 &pt_world) { MatXX A(2 * poses.size(), 4); VecX b(2 * poses.size()); b.setZero(); for (size_t i = 0; i < poses.size(); ++i) { Mat34 m = poses[i].matrix3x4(); A.block<1, 4>(2 * i, 0) = points[i][0] * m.row(2) - m.row(0); A.block<1, 4>(2 * i + 1, 0) = points[i][1] * m.row(2) - m.row(1); } auto svd = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); pt_world = (svd.matrixV().col(3) / svd.matrixV()(3, 3)).head<3>(); if (svd.singularValues()[3] / svd.singularValues()[2] < 1e-2) { // 解质量不好，放弃 return true; } return false; } // converters inline Vec2 toVec2(const cv::Point2f p) { return Vec2(p.x, p.y); } } // namespace myslam #endif // MYSLAM_ALGORITHM_H ================================================ FILE: code/ch12/myslam/include/myslam/backend.h ================================================ // // Created by gaoxiang on 19-5-2. // #ifndef MYSLAM_BACKEND_H #define MYSLAM_BACKEND_H #include "myslam/common_include.h" #include "myslam/frame.h" #include "myslam/map.h" namespace myslam { class Map; /** * 后端 * 有单独优化线程，在Map更新时启动优化 * Map更新由前端触发 */ class Backend { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; /// 构造函数中启动优化线程并挂起 Backend(); // 设置左右目的相机，用于获得内外参 void SetCameras(Camera::Ptr left, Camera::Ptr right) { cam_left_ = left; cam_right_ = right; } /// 设置地图 void SetMap(std::shared_ptr map) { map_ = map; } /// 触发地图更新，启动优化 void UpdateMap(); /// 关闭后端线程 void Stop(); private: /// 后端线程 void BackendLoop(); /// 对给定关键帧和路标点进行优化 void Optimize(Map::KeyframesType& keyframes, Map::LandmarksType& landmarks); std::shared_ptr map_; std::thread backend_thread_; std::mutex data_mutex_; std::condition_variable map_update_; std::atomic backend_running_; Camera::Ptr cam_left_ = nullptr, cam_right_ = nullptr; }; } // namespace myslam #endif // MYSLAM_BACKEND_H ================================================ FILE: code/ch12/myslam/include/myslam/camera.h ================================================ #pragma once #ifndef MYSLAM_CAMERA_H #define MYSLAM_CAMERA_H #include "myslam/common_include.h" namespace myslam { // Pinhole stereo camera model class Camera { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; double fx_ = 0, fy_ = 0, cx_ = 0, cy_ = 0, baseline_ = 0; // Camera intrinsics SE3 pose_; // extrinsic, from stereo camera to single camera SE3 pose_inv_; // inverse of extrinsics Camera(); Camera(double fx, double fy, double cx, double cy, double baseline, const SE3 &pose) : fx_(fx), fy_(fy), cx_(cx), cy_(cy), baseline_(baseline), pose_(pose) { pose_inv_ = pose_.inverse(); } SE3 pose() const { return pose_; } // return intrinsic matrix Mat33 K() const { Mat33 k; k << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1; return k; } // coordinate transform: world, camera, pixel Vec3 world2camera(const Vec3 &p_w, const SE3 &T_c_w); Vec3 camera2world(const Vec3 &p_c, const SE3 &T_c_w); Vec2 camera2pixel(const Vec3 &p_c); Vec3 pixel2camera(const Vec2 &p_p, double depth = 1); Vec3 pixel2world(const Vec2 &p_p, const SE3 &T_c_w, double depth = 1); Vec2 world2pixel(const Vec3 &p_w, const SE3 &T_c_w); }; } // namespace myslam #endif // MYSLAM_CAMERA_H ================================================ FILE: code/ch12/myslam/include/myslam/common_include.h ================================================ #pragma once #ifndef MYSLAM_COMMON_INCLUDE_H #define MYSLAM_COMMON_INCLUDE_H // std #include #include #include #include #include #include #include #include #include #include #include #include #include // define the commonly included file to avoid a long include list #include #include // typedefs for eigen // double matricies typedef Eigen::Matrix MatXX; typedef Eigen::Matrix Mat1010; typedef Eigen::Matrix Mat1313; typedef Eigen::Matrix Mat810; typedef Eigen::Matrix Mat83; typedef Eigen::Matrix Mat66; typedef Eigen::Matrix Mat53; typedef Eigen::Matrix Mat43; typedef Eigen::Matrix Mat42; typedef Eigen::Matrix Mat33; typedef Eigen::Matrix Mat22; typedef Eigen::Matrix Mat88; typedef Eigen::Matrix Mat77; typedef Eigen::Matrix Mat49; typedef Eigen::Matrix Mat89; typedef Eigen::Matrix Mat94; typedef Eigen::Matrix Mat98; typedef Eigen::Matrix Mat81; typedef Eigen::Matrix Mat18; typedef Eigen::Matrix Mat91; typedef Eigen::Matrix Mat19; typedef Eigen::Matrix Mat84; typedef Eigen::Matrix Mat48; typedef Eigen::Matrix Mat44; typedef Eigen::Matrix Mat34; typedef Eigen::Matrix Mat1414; // float matricies typedef Eigen::Matrix Mat33f; typedef Eigen::Matrix Mat103f; typedef Eigen::Matrix Mat22f; typedef Eigen::Matrix Vec3f; typedef Eigen::Matrix Vec2f; typedef Eigen::Matrix Vec6f; typedef Eigen::Matrix Mat18f; typedef Eigen::Matrix Mat66f; typedef Eigen::Matrix Mat88f; typedef Eigen::Matrix Mat84f; typedef Eigen::Matrix Mat66f; typedef Eigen::Matrix Mat44f; typedef Eigen::Matrix Mat1212f; typedef Eigen::Matrix Mat1313f; typedef Eigen::Matrix Mat1010f; typedef Eigen::Matrix Mat99f; typedef Eigen::Matrix Mat42f; typedef Eigen::Matrix Mat62f; typedef Eigen::Matrix Mat12f; typedef Eigen::Matrix MatXXf; typedef Eigen::Matrix Mat1414f; // double vectors typedef Eigen::Matrix Vec14; typedef Eigen::Matrix Vec13; typedef Eigen::Matrix Vec10; typedef Eigen::Matrix Vec9; typedef Eigen::Matrix Vec8; typedef Eigen::Matrix Vec7; typedef Eigen::Matrix Vec6; typedef Eigen::Matrix Vec5; typedef Eigen::Matrix Vec4; typedef Eigen::Matrix Vec3; typedef Eigen::Matrix Vec2; typedef Eigen::Matrix VecX; // float vectors typedef Eigen::Matrix Vec12f; typedef Eigen::Matrix Vec8f; typedef Eigen::Matrix Vec10f; typedef Eigen::Matrix Vec4f; typedef Eigen::Matrix Vec12f; typedef Eigen::Matrix Vec13f; typedef Eigen::Matrix Vec9f; typedef Eigen::Matrix VecXf; typedef Eigen::Matrix Vec14f; // for Sophus #include #include typedef Sophus::SE3d SE3; typedef Sophus::SO3d SO3; // for cv #include using cv::Mat; // glog #include #endif // MYSLAM_COMMON_INCLUDE_H ================================================ FILE: code/ch12/myslam/include/myslam/config.h ================================================ #pragma once #ifndef MYSLAM_CONFIG_H #define MYSLAM_CONFIG_H #include "myslam/common_include.h" namespace myslam { /** * 配置类，使用SetParameterFile确定配置文件 * 然后用Get得到对应值 * 单例模式 */ class Config { private: static std::shared_ptr config_; cv::FileStorage file_; Config() {} // private constructor makes a singleton public: ~Config(); // close the file when deconstructing // set a new config file static bool SetParameterFile(const std::string &filename); // access the parameter values template static T Get(const std::string &key) { return T(Config::config_->file_[key]); } }; } // namespace myslam #endif // MYSLAM_CONFIG_H ================================================ FILE: code/ch12/myslam/include/myslam/dataset.h ================================================ #ifndef MYSLAM_DATASET_H #define MYSLAM_DATASET_H #include "myslam/camera.h" #include "myslam/common_include.h" #include "myslam/frame.h" namespace myslam { /** * 数据集读取 * 构造时传入配置文件路径，配置文件的dataset_dir为数据集路径 * Init之后可获得相机和下一帧图像 */ class Dataset { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; Dataset(const std::string& dataset_path); /// 初始化，返回是否成功 bool Init(); /// create and return the next frame containing the stereo images Frame::Ptr NextFrame(); /// get camera by id Camera::Ptr GetCamera(int camera_id) const { return cameras_.at(camera_id); } private: std::string dataset_path_; int current_image_index_ = 0; std::vector cameras_; }; } // namespace myslam #endif ================================================ FILE: code/ch12/myslam/include/myslam/feature.h ================================================ // // Created by gaoxiang on 19-5-2. // #pragma once #ifndef MYSLAM_FEATURE_H #define MYSLAM_FEATURE_H #include #include #include "myslam/common_include.h" namespace myslam { struct Frame; struct MapPoint; /** * 2D 特征点 * 在三角化之后会被关联一个地图点 */ struct Feature { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; std::weak_ptr frame_; // 持有该feature的frame cv::KeyPoint position_; // 2D提取位置 std::weak_ptr map_point_; // 关联地图点 bool is_outlier_ = false; // 是否为异常点 bool is_on_left_image_ = true; // 标识是否提在左图，false为右图 public: Feature() {} Feature(std::shared_ptr frame, const cv::KeyPoint &kp) : frame_(frame), position_(kp) {} }; } // namespace myslam #endif // MYSLAM_FEATURE_H ================================================ FILE: code/ch12/myslam/include/myslam/frame.h ================================================ #pragma once #ifndef MYSLAM_FRAME_H #define MYSLAM_FRAME_H #include "myslam/camera.h" #include "myslam/common_include.h" namespace myslam { // forward declare struct MapPoint; struct Feature; /** * 帧 * 每一帧分配独立id，关键帧分配关键帧ID */ struct Frame { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; unsigned long id_ = 0; // id of this frame unsigned long keyframe_id_ = 0; // id of key frame bool is_keyframe_ = false; // 是否为关键帧 double time_stamp_; // 时间戳，暂不使用 SE3 pose_; // Tcw 形式Pose std::mutex pose_mutex_; // Pose数据锁 cv::Mat left_img_, right_img_; // stereo images // extracted features in left image std::vector> features_left_; // corresponding features in right image, set to nullptr if no corresponding std::vector> features_right_; public: // data members Frame() {} Frame(long id, double time_stamp, const SE3 &pose, const Mat &left, const Mat &right); // set and get pose, thread safe SE3 Pose() { std::unique_lock lck(pose_mutex_); return pose_; } void SetPose(const SE3 &pose) { std::unique_lock lck(pose_mutex_); pose_ = pose; } /// 设置关键帧并分配并键帧id void SetKeyFrame(); /// 工厂构建模式，分配id static std::shared_ptr CreateFrame(); }; } // namespace myslam #endif // MYSLAM_FRAME_H ================================================ FILE: code/ch12/myslam/include/myslam/frontend.h ================================================ #pragma once #ifndef MYSLAM_FRONTEND_H #define MYSLAM_FRONTEND_H #include #include "myslam/common_include.h" #include "myslam/frame.h" #include "myslam/map.h" namespace myslam { class Backend; class Viewer; enum class FrontendStatus { INITING, TRACKING_GOOD, TRACKING_BAD, LOST }; /** * 前端 * 估计当前帧Pose，在满足关键帧条件时向地图加入关键帧并触发优化 */ class Frontend { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; Frontend(); /// 外部接口，添加一个帧并计算其定位结果 bool AddFrame(Frame::Ptr frame); /// Set函数 void SetMap(Map::Ptr map) { map_ = map; } void SetBackend(std::shared_ptr backend) { backend_ = backend; } void SetViewer(std::shared_ptr viewer) { viewer_ = viewer; } FrontendStatus GetStatus() const { return status_; } void SetCameras(Camera::Ptr left, Camera::Ptr right) { camera_left_ = left; camera_right_ = right; } private: /** * Track in normal mode * @return true if success */ bool Track(); /** * Reset when lost * @return true if success */ bool Reset(); /** * Track with last frame * @return num of tracked points */ int TrackLastFrame(); /** * estimate current frame's pose * @return num of inliers */ int EstimateCurrentPose(); /** * set current frame as a keyframe and insert it into backend * @return true if success */ bool InsertKeyframe(); /** * Try init the frontend with stereo images saved in current_frame_ * @return true if success */ bool StereoInit(); /** * Detect features in left image in current_frame_ * keypoints will be saved in current_frame_ * @return */ int DetectFeatures(); /** * Find the corresponding features in right image of current_frame_ * @return num of features found */ int FindFeaturesInRight(); /** * Build the initial map with single image * @return true if succeed */ bool BuildInitMap(); /** * Triangulate the 2D points in current frame * @return num of triangulated points */ int TriangulateNewPoints(); /** * Set the features in keyframe as new observation of the map points */ void SetObservationsForKeyFrame(); // data FrontendStatus status_ = FrontendStatus::INITING; Frame::Ptr current_frame_ = nullptr; // 当前帧 Frame::Ptr last_frame_ = nullptr; // 上一帧 Camera::Ptr camera_left_ = nullptr; // 左侧相机 Camera::Ptr camera_right_ = nullptr; // 右侧相机 Map::Ptr map_ = nullptr; std::shared_ptr backend_ = nullptr; std::shared_ptr viewer_ = nullptr; SE3 relative_motion_; // 当前帧与上一帧的相对运动，用于估计当前帧pose初值 int tracking_inliers_ = 0; // inliers, used for testing new keyframes // params int num_features_ = 200; int num_features_init_ = 100; int num_features_tracking_ = 50; int num_features_tracking_bad_ = 20; int num_features_needed_for_keyframe_ = 80; // utilities cv::Ptr gftt_; // feature detector in opencv }; } // namespace myslam #endif // MYSLAM_FRONTEND_H ================================================ FILE: code/ch12/myslam/include/myslam/g2o_types.h ================================================ // // Created by gaoxiang on 19-5-4. // #ifndef MYSLAM_G2O_TYPES_H #define MYSLAM_G2O_TYPES_H #include "myslam/common_include.h" #include #include #include #include #include #include #include #include #include #include #include namespace myslam { /// vertex and edges used in g2o ba /// 位姿顶点 class VertexPose : public g2o::BaseVertex<6, SE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; virtual void setToOriginImpl() override { _estimate = SE3(); } /// left multiplication on SE3 virtual void oplusImpl(const double *update) override { Vec6 update_eigen; update_eigen << update[0], update[1], update[2], update[3], update[4], update[5]; _estimate = SE3::exp(update_eigen) * _estimate; } virtual bool read(std::istream &in) override { return true; } virtual bool write(std::ostream &out) const override { return true; } }; /// 路标顶点 class VertexXYZ : public g2o::BaseVertex<3, Vec3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; virtual void setToOriginImpl() override { _estimate = Vec3::Zero(); } virtual void oplusImpl(const double *update) override { _estimate[0] += update[0]; _estimate[1] += update[1]; _estimate[2] += update[2]; } virtual bool read(std::istream &in) override { return true; } virtual bool write(std::ostream &out) const override { return true; } }; /// 仅估计位姿的一元边 class EdgeProjectionPoseOnly : public g2o::BaseUnaryEdge<2, Vec2, VertexPose> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; EdgeProjectionPoseOnly(const Vec3 &pos, const Mat33 &K) : _pos3d(pos), _K(K) {} virtual void computeError() override { const VertexPose *v = static_cast(_vertices[0]); SE3 T = v->estimate(); Vec3 pos_pixel = _K * (T * _pos3d); pos_pixel /= pos_pixel[2]; _error = _measurement - pos_pixel.head<2>(); } virtual void linearizeOplus() override { const VertexPose *v = static_cast(_vertices[0]); SE3 T = v->estimate(); Vec3 pos_cam = T * _pos3d; double fx = _K(0, 0); double fy = _K(1, 1); double X = pos_cam[0]; double Y = pos_cam[1]; double Z = pos_cam[2]; double Zinv = 1.0 / (Z + 1e-18); double Zinv2 = Zinv * Zinv; _jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2, -fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv, fy * Y * Zinv2, fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2, -fy * X * Zinv; } virtual bool read(std::istream &in) override { return true; } virtual bool write(std::ostream &out) const override { return true; } private: Vec3 _pos3d; Mat33 _K; }; /// 带有地图和位姿的二元边 class EdgeProjection : public g2o::BaseBinaryEdge<2, Vec2, VertexPose, VertexXYZ> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; /// 构造时传入相机内外参 EdgeProjection(const Mat33 &K, const SE3 &cam_ext) : _K(K) { _cam_ext = cam_ext; } virtual void computeError() override { const VertexPose *v0 = static_cast(_vertices[0]); const VertexXYZ *v1 = static_cast(_vertices[1]); SE3 T = v0->estimate(); Vec3 pos_pixel = _K * (_cam_ext * (T * v1->estimate())); pos_pixel /= pos_pixel[2]; _error = _measurement - pos_pixel.head<2>(); } virtual void linearizeOplus() override { const VertexPose *v0 = static_cast(_vertices[0]); const VertexXYZ *v1 = static_cast(_vertices[1]); SE3 T = v0->estimate(); Vec3 pw = v1->estimate(); Vec3 pos_cam = _cam_ext * T * pw; double fx = _K(0, 0); double fy = _K(1, 1); double X = pos_cam[0]; double Y = pos_cam[1]; double Z = pos_cam[2]; double Zinv = 1.0 / (Z + 1e-18); double Zinv2 = Zinv * Zinv; _jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2, -fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv, fy * Y * Zinv2, fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2, -fy * X * Zinv; _jacobianOplusXj = _jacobianOplusXi.block<2, 3>(0, 0) * _cam_ext.rotationMatrix() * T.rotationMatrix(); } virtual bool read(std::istream &in) override { return true; } virtual bool write(std::ostream &out) const override { return true; } private: Mat33 _K; SE3 _cam_ext; }; } // namespace myslam #endif // MYSLAM_G2O_TYPES_H ================================================ FILE: code/ch12/myslam/include/myslam/map.h ================================================ #pragma once #ifndef MAP_H #define MAP_H #include "myslam/common_include.h" #include "myslam/frame.h" #include "myslam/mappoint.h" namespace myslam { /** * @brief 地图 * 和地图的交互：前端调用InsertKeyframe和InsertMapPoint插入新帧和地图点，后端维护地图的结构，判定outlier/剔除等等 */ class Map { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; typedef std::unordered_map LandmarksType; typedef std::unordered_map KeyframesType; Map() {} /// 增加一个关键帧 void InsertKeyFrame(Frame::Ptr frame); /// 增加一个地图顶点 void InsertMapPoint(MapPoint::Ptr map_point); /// 获取所有地图点 LandmarksType GetAllMapPoints() { std::unique_lock lck(data_mutex_); return landmarks_; } /// 获取所有关键帧 KeyframesType GetAllKeyFrames() { std::unique_lock lck(data_mutex_); return keyframes_; } /// 获取激活地图点 LandmarksType GetActiveMapPoints() { std::unique_lock lck(data_mutex_); return active_landmarks_; } /// 获取激活关键帧 KeyframesType GetActiveKeyFrames() { std::unique_lock lck(data_mutex_); return active_keyframes_; } /// 清理map中观测数量为零的点 void CleanMap(); private: // 将旧的关键帧置为不活跃状态 void RemoveOldKeyframe(); std::mutex data_mutex_; LandmarksType landmarks_; // all landmarks LandmarksType active_landmarks_; // active landmarks KeyframesType keyframes_; // all key-frames KeyframesType active_keyframes_; // all key-frames Frame::Ptr current_frame_ = nullptr; // settings int num_active_keyframes_ = 7; // 激活的关键帧数量 }; } // namespace myslam #endif // MAP_H ================================================ FILE: code/ch12/myslam/include/myslam/mappoint.h ================================================ #pragma once #ifndef MYSLAM_MAPPOINT_H #define MYSLAM_MAPPOINT_H #include "myslam/common_include.h" namespace myslam { struct Frame; struct Feature; /** * 路标点类 * 特征点在三角化之后形成路标点 */ struct MapPoint { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; unsigned long id_ = 0; // ID bool is_outlier_ = false; Vec3 pos_ = Vec3::Zero(); // Position in world std::mutex data_mutex_; int observed_times_ = 0; // being observed by feature matching algo. std::list> observations_; MapPoint() {} MapPoint(long id, Vec3 position); Vec3 Pos() { std::unique_lock lck(data_mutex_); return pos_; } void SetPos(const Vec3 &pos) { std::unique_lock lck(data_mutex_); pos_ = pos; }; void AddObservation(std::shared_ptr feature) { std::unique_lock lck(data_mutex_); observations_.push_back(feature); observed_times_++; } void RemoveObservation(std::shared_ptr feat); std::list> GetObs() { std::unique_lock lck(data_mutex_); return observations_; } // factory function static MapPoint::Ptr CreateNewMappoint(); }; } // namespace myslam #endif // MYSLAM_MAPPOINT_H ================================================ FILE: code/ch12/myslam/include/myslam/viewer.h ================================================ // // Created by gaoxiang on 19-5-4. // #ifndef MYSLAM_VIEWER_H #define MYSLAM_VIEWER_H #include #include #include "myslam/common_include.h" #include "myslam/frame.h" #include "myslam/map.h" namespace myslam { /** * 可视化 */ class Viewer { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; Viewer(); void SetMap(Map::Ptr map) { map_ = map; } void Close(); // 增加一个当前帧 void AddCurrentFrame(Frame::Ptr current_frame); // 更新地图 void UpdateMap(); private: void ThreadLoop(); void DrawFrame(Frame::Ptr frame, const float* color); void DrawMapPoints(); void FollowCurrentFrame(pangolin::OpenGlRenderState& vis_camera); /// plot the features in current frame into an image cv::Mat PlotFrameImage(); Frame::Ptr current_frame_ = nullptr; Map::Ptr map_ = nullptr; std::thread viewer_thread_; bool viewer_running_ = true; std::unordered_map active_keyframes_; std::unordered_map active_landmarks_; bool map_updated_ = false; std::mutex viewer_data_mutex_; }; } // namespace myslam #endif // MYSLAM_VIEWER_H ================================================ FILE: code/ch12/myslam/include/myslam/visual_odometry.h ================================================ #pragma once #ifndef MYSLAM_VISUAL_ODOMETRY_H #define MYSLAM_VISUAL_ODOMETRY_H #include "myslam/backend.h" #include "myslam/common_include.h" #include "myslam/dataset.h" #include "myslam/frontend.h" #include "myslam/viewer.h" namespace myslam { /** * VO 对外接口 */ class VisualOdometry { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; typedef std::shared_ptr Ptr; /// constructor with config file VisualOdometry(std::string &config_path); /** * do initialization things before run * @return true if success */ bool Init(); /** * start vo in the dataset */ void Run(); /** * Make a step forward in dataset */ bool Step(); /// 获取前端状态 FrontendStatus GetFrontendStatus() const { return frontend_->GetStatus(); } private: bool inited_ = false; std::string config_file_path_; Frontend::Ptr frontend_ = nullptr; Backend::Ptr backend_ = nullptr; Map::Ptr map_ = nullptr; Viewer::Ptr viewer_ = nullptr; // dataset Dataset::Ptr dataset_ = nullptr; }; } // namespace myslam #endif // MYSLAM_VISUAL_ODOMETRY_H ================================================ FILE: code/ch12/myslam/src/CMakeLists.txt ================================================ add_library(myslam SHARED frame.cpp mappoint.cpp map.cpp camera.cpp config.cpp feature.cpp frontend.cpp backend.cpp viewer.cpp visual_odometry.cpp dataset.cpp) target_link_libraries(myslam ${THIRD_PARTY_LIBS} fmt::fmt) ================================================ FILE: code/ch12/myslam/src/backend.cpp ================================================ // // Created by gaoxiang on 19-5-2. // #include "myslam/backend.h" #include "myslam/algorithm.h" #include "myslam/feature.h" #include "myslam/g2o_types.h" #include "myslam/map.h" #include "myslam/mappoint.h" namespace myslam { Backend::Backend() { backend_running_.store(true); backend_thread_ = std::thread(std::bind(&Backend::BackendLoop, this)); } void Backend::UpdateMap() { std::unique_lock lock(data_mutex_); map_update_.notify_one(); } void Backend::Stop() { backend_running_.store(false); map_update_.notify_one(); backend_thread_.join(); } void Backend::BackendLoop() { while (backend_running_.load()) { std::unique_lock lock(data_mutex_); map_update_.wait(lock); /// 后端仅优化激活的Frames和Landmarks Map::KeyframesType active_kfs = map_->GetActiveKeyFrames(); Map::LandmarksType active_landmarks = map_->GetActiveMapPoints(); Optimize(active_kfs, active_landmarks); } } void Backend::Optimize(Map::KeyframesType &keyframes, Map::LandmarksType &landmarks) { // setup g2o typedef g2o::BlockSolver_6_3 BlockSolverType; typedef g2o::LinearSolverCSparse LinearSolverType; auto solver = new g2o::OptimizationAlgorithmLevenberg( g2o::make_unique( g2o::make_unique())); g2o::SparseOptimizer optimizer; optimizer.setAlgorithm(solver); // pose 顶点，使用Keyframe id std::map vertices; unsigned long max_kf_id = 0; for (auto &keyframe : keyframes) { auto kf = keyframe.second; VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose vertex_pose->setId(kf->keyframe_id_); vertex_pose->setEstimate(kf->Pose()); optimizer.addVertex(vertex_pose); if (kf->keyframe_id_ > max_kf_id) { max_kf_id = kf->keyframe_id_; } vertices.insert({kf->keyframe_id_, vertex_pose}); } // 路标顶点，使用路标id索引 std::map vertices_landmarks; // K 和左右外参 Mat33 K = cam_left_->K(); SE3 left_ext = cam_left_->pose(); SE3 right_ext = cam_right_->pose(); // edges int index = 1; double chi2_th = 5.991; // robust kernel 阈值 std::map edges_and_features; for (auto &landmark : landmarks) { if (landmark.second->is_outlier_) continue; unsigned long landmark_id = landmark.second->id_; auto observations = landmark.second->GetObs(); for (auto &obs : observations) { if (obs.lock() == nullptr) continue; auto feat = obs.lock(); if (feat->is_outlier_ || feat->frame_.lock() == nullptr) continue; auto frame = feat->frame_.lock(); EdgeProjection *edge = nullptr; if (feat->is_on_left_image_) { edge = new EdgeProjection(K, left_ext); } else { edge = new EdgeProjection(K, right_ext); } // 如果landmark还没有被加入优化，则新加一个顶点 if (vertices_landmarks.find(landmark_id) == vertices_landmarks.end()) { VertexXYZ *v = new VertexXYZ; v->setEstimate(landmark.second->Pos()); v->setId(landmark_id + max_kf_id + 1); v->setMarginalized(true); vertices_landmarks.insert({landmark_id, v}); optimizer.addVertex(v); } edge->setId(index); edge->setVertex(0, vertices.at(frame->keyframe_id_)); // pose edge->setVertex(1, vertices_landmarks.at(landmark_id)); // landmark edge->setMeasurement(toVec2(feat->position_.pt)); edge->setInformation(Mat22::Identity()); auto rk = new g2o::RobustKernelHuber(); rk->setDelta(chi2_th); edge->setRobustKernel(rk); edges_and_features.insert({edge, feat}); optimizer.addEdge(edge); index++; } } // do optimization and eliminate the outliers optimizer.initializeOptimization(); optimizer.optimize(10); int cnt_outlier = 0, cnt_inlier = 0; int iteration = 0; while (iteration < 5) { cnt_outlier = 0; cnt_inlier = 0; // determine if we want to adjust the outlier threshold for (auto &ef : edges_and_features) { if (ef.first->chi2() > chi2_th) { cnt_outlier++; } else { cnt_inlier++; } } double inlier_ratio = cnt_inlier / double(cnt_inlier + cnt_outlier); if (inlier_ratio > 0.5) { break; } else { chi2_th *= 2; iteration++; } } for (auto &ef : edges_and_features) { if (ef.first->chi2() > chi2_th) { ef.second->is_outlier_ = true; // remove the observation ef.second->map_point_.lock()->RemoveObservation(ef.second); } else { ef.second->is_outlier_ = false; } } LOG(INFO) << "Outlier/Inlier in optimization: " << cnt_outlier << "/" << cnt_inlier; // Set pose and lanrmark position for (auto &v : vertices) { keyframes.at(v.first)->SetPose(v.second->estimate()); } for (auto &v : vertices_landmarks) { landmarks.at(v.first)->SetPos(v.second->estimate()); } } } // namespace myslam ================================================ FILE: code/ch12/myslam/src/camera.cpp ================================================ #include "myslam/camera.h" namespace myslam { Camera::Camera() { } Vec3 Camera::world2camera(const Vec3 &p_w, const SE3 &T_c_w) { return pose_ * T_c_w * p_w; } Vec3 Camera::camera2world(const Vec3 &p_c, const SE3 &T_c_w) { return T_c_w.inverse() * pose_inv_ * p_c; } Vec2 Camera::camera2pixel(const Vec3 &p_c) { return Vec2( fx_ * p_c(0, 0) / p_c(2, 0) + cx_, fy_ * p_c(1, 0) / p_c(2, 0) + cy_ ); } Vec3 Camera::pixel2camera(const Vec2 &p_p, double depth) { return Vec3( (p_p(0, 0) - cx_) * depth / fx_, (p_p(1, 0) - cy_) * depth / fy_, depth ); } Vec2 Camera::world2pixel(const Vec3 &p_w, const SE3 &T_c_w) { return camera2pixel(world2camera(p_w, T_c_w)); } Vec3 Camera::pixel2world(const Vec2 &p_p, const SE3 &T_c_w, double depth) { return camera2world(pixel2camera(p_p, depth), T_c_w); } } ================================================ FILE: code/ch12/myslam/src/config.cpp ================================================ #include "myslam/config.h" namespace myslam { bool Config::SetParameterFile(const std::string &filename) { if (config_ == nullptr) config_ = std::shared_ptr(new Config); config_->file_ = cv::FileStorage(filename.c_str(), cv::FileStorage::READ); if (config_->file_.isOpened() == false) { LOG(ERROR) << "parameter file " << filename << " does not exist."; config_->file_.release(); return false; } return true; } Config::~Config() { if (file_.isOpened()) file_.release(); } std::shared_ptr Config::config_ = nullptr; } ================================================ FILE: code/ch12/myslam/src/dataset.cpp ================================================ #include "myslam/dataset.h" #include "myslam/frame.h" #include #include #include using namespace std; namespace myslam { Dataset::Dataset(const std::string& dataset_path) : dataset_path_(dataset_path) {} bool Dataset::Init() { // read camera intrinsics and extrinsics ifstream fin(dataset_path_ + "/calib.txt"); if (!fin) { LOG(ERROR) << "cannot find " << dataset_path_ << "/calib.txt!"; return false; } for (int i = 0; i < 4; ++i) { char camera_name[3]; for (int k = 0; k < 3; ++k) { fin >> camera_name[k]; } double projection_data[12]; for (int k = 0; k < 12; ++k) { fin >> projection_data[k]; } Mat33 K; K << projection_data[0], projection_data[1], projection_data[2], projection_data[4], projection_data[5], projection_data[6], projection_data[8], projection_data[9], projection_data[10]; Vec3 t; t << projection_data[3], projection_data[7], projection_data[11]; t = K.inverse() * t; K = K * 0.5; Camera::Ptr new_camera(new Camera(K(0, 0), K(1, 1), K(0, 2), K(1, 2), t.norm(), SE3(SO3(), t))); cameras_.push_back(new_camera); LOG(INFO) << "Camera " << i << " extrinsics: " << t.transpose(); } fin.close(); current_image_index_ = 0; return true; } Frame::Ptr Dataset::NextFrame() { boost::format fmt("%s/image_%d/%06d.png"); cv::Mat image_left, image_right; // read images image_left = cv::imread((fmt % dataset_path_ % 0 % current_image_index_).str(), cv::IMREAD_GRAYSCALE); image_right = cv::imread((fmt % dataset_path_ % 1 % current_image_index_).str(), cv::IMREAD_GRAYSCALE); if (image_left.data == nullptr || image_right.data == nullptr) { LOG(WARNING) << "cannot find images at index " << current_image_index_; return nullptr; } cv::Mat image_left_resized, image_right_resized; cv::resize(image_left, image_left_resized, cv::Size(), 0.5, 0.5, cv::INTER_NEAREST); cv::resize(image_right, image_right_resized, cv::Size(), 0.5, 0.5, cv::INTER_NEAREST); auto new_frame = Frame::CreateFrame(); new_frame->left_img_ = image_left_resized; new_frame->right_img_ = image_right_resized; current_image_index_++; return new_frame; } } // namespace myslam ================================================ FILE: code/ch12/myslam/src/feature.cpp ================================================ // // Created by gaoxiang on 19-5-2. // #include "myslam/feature.h" namespace myslam { } ================================================ FILE: code/ch12/myslam/src/frame.cpp ================================================ /* * * Copyright (C) 2016 * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * */ #include "myslam/frame.h" namespace myslam { Frame::Frame(long id, double time_stamp, const SE3 &pose, const Mat &left, const Mat &right) : id_(id), time_stamp_(time_stamp), pose_(pose), left_img_(left), right_img_(right) {} Frame::Ptr Frame::CreateFrame() { static long factory_id = 0; Frame::Ptr new_frame(new Frame); new_frame->id_ = factory_id++; return new_frame; } void Frame::SetKeyFrame() { static long keyframe_factory_id = 0; is_keyframe_ = true; keyframe_id_ = keyframe_factory_id++; } } ================================================ FILE: code/ch12/myslam/src/frontend.cpp ================================================ // // Created by gaoxiang on 19-5-2. // #include #include "myslam/algorithm.h" #include "myslam/backend.h" #include "myslam/config.h" #include "myslam/feature.h" #include "myslam/frontend.h" #include "myslam/g2o_types.h" #include "myslam/map.h" #include "myslam/viewer.h" namespace myslam { Frontend::Frontend() { gftt_ = cv::GFTTDetector::create(Config::Get("num_features"), 0.01, 20); num_features_init_ = Config::Get("num_features_init"); num_features_ = Config::Get("num_features"); } bool Frontend::AddFrame(myslam::Frame::Ptr frame) { current_frame_ = frame; switch (status_) { case FrontendStatus::INITING: StereoInit(); break; case FrontendStatus::TRACKING_GOOD: case FrontendStatus::TRACKING_BAD: Track(); break; case FrontendStatus::LOST: Reset(); break; } last_frame_ = current_frame_; return true; } bool Frontend::Track() { if (last_frame_) { current_frame_->SetPose(relative_motion_ * last_frame_->Pose()); } int num_track_last = TrackLastFrame(); tracking_inliers_ = EstimateCurrentPose(); if (tracking_inliers_ > num_features_tracking_) { // tracking good status_ = FrontendStatus::TRACKING_GOOD; } else if (tracking_inliers_ > num_features_tracking_bad_) { // tracking bad status_ = FrontendStatus::TRACKING_BAD; } else { // lost status_ = FrontendStatus::LOST; } InsertKeyframe(); relative_motion_ = current_frame_->Pose() * last_frame_->Pose().inverse(); if (viewer_) viewer_->AddCurrentFrame(current_frame_); return true; } bool Frontend::InsertKeyframe() { if (tracking_inliers_ >= num_features_needed_for_keyframe_) { // still have enough features, don't insert keyframe return false; } // current frame is a new keyframe current_frame_->SetKeyFrame(); map_->InsertKeyFrame(current_frame_); LOG(INFO) << "Set frame " << current_frame_->id_ << " as keyframe " << current_frame_->keyframe_id_; SetObservationsForKeyFrame(); DetectFeatures(); // detect new features // track in right image FindFeaturesInRight(); // triangulate map points TriangulateNewPoints(); // update backend because we have a new keyframe backend_->UpdateMap(); if (viewer_) viewer_->UpdateMap(); return true; } void Frontend::SetObservationsForKeyFrame() { for (auto &feat : current_frame_->features_left_) { auto mp = feat->map_point_.lock(); if (mp) mp->AddObservation(feat); } } int Frontend::TriangulateNewPoints() { std::vector poses{camera_left_->pose(), camera_right_->pose()}; SE3 current_pose_Twc = current_frame_->Pose().inverse(); int cnt_triangulated_pts = 0; for (size_t i = 0; i < current_frame_->features_left_.size(); ++i) { if (current_frame_->features_left_[i]->map_point_.expired() && current_frame_->features_right_[i] != nullptr) { // 左图的特征点未关联地图点且存在右图匹配点，尝试三角化 std::vector points{ camera_left_->pixel2camera( Vec2(current_frame_->features_left_[i]->position_.pt.x, current_frame_->features_left_[i]->position_.pt.y)), camera_right_->pixel2camera( Vec2(current_frame_->features_right_[i]->position_.pt.x, current_frame_->features_right_[i]->position_.pt.y))}; Vec3 pworld = Vec3::Zero(); if (triangulation(poses, points, pworld) && pworld[2] > 0) { auto new_map_point = MapPoint::CreateNewMappoint(); pworld = current_pose_Twc * pworld; new_map_point->SetPos(pworld); new_map_point->AddObservation( current_frame_->features_left_[i]); new_map_point->AddObservation( current_frame_->features_right_[i]); current_frame_->features_left_[i]->map_point_ = new_map_point; current_frame_->features_right_[i]->map_point_ = new_map_point; map_->InsertMapPoint(new_map_point); cnt_triangulated_pts++; } } } LOG(INFO) << "new landmarks: " << cnt_triangulated_pts; return cnt_triangulated_pts; } int Frontend::EstimateCurrentPose() { // setup g2o typedef g2o::BlockSolver_6_3 BlockSolverType; typedef g2o::LinearSolverDense LinearSolverType; auto solver = new g2o::OptimizationAlgorithmLevenberg( g2o::make_unique( g2o::make_unique())); g2o::SparseOptimizer optimizer; optimizer.setAlgorithm(solver); // vertex VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose vertex_pose->setId(0); vertex_pose->setEstimate(current_frame_->Pose()); optimizer.addVertex(vertex_pose); // K Mat33 K = camera_left_->K(); // edges int index = 1; std::vector edges; std::vector features; for (size_t i = 0; i < current_frame_->features_left_.size(); ++i) { auto mp = current_frame_->features_left_[i]->map_point_.lock(); if (mp) { features.push_back(current_frame_->features_left_[i]); EdgeProjectionPoseOnly *edge = new EdgeProjectionPoseOnly(mp->pos_, K); edge->setId(index); edge->setVertex(0, vertex_pose); edge->setMeasurement( toVec2(current_frame_->features_left_[i]->position_.pt)); edge->setInformation(Eigen::Matrix2d::Identity()); edge->setRobustKernel(new g2o::RobustKernelHuber); edges.push_back(edge); optimizer.addEdge(edge); index++; } } // estimate the Pose the determine the outliers const double chi2_th = 5.991; int cnt_outlier = 0; for (int iteration = 0; iteration < 4; ++iteration) { vertex_pose->setEstimate(current_frame_->Pose()); optimizer.initializeOptimization(); optimizer.optimize(10); cnt_outlier = 0; // count the outliers for (size_t i = 0; i < edges.size(); ++i) { auto e = edges[i]; if (features[i]->is_outlier_) { e->computeError(); } if (e->chi2() > chi2_th) { features[i]->is_outlier_ = true; e->setLevel(1); cnt_outlier++; } else { features[i]->is_outlier_ = false; e->setLevel(0); }; if (iteration == 2) { e->setRobustKernel(nullptr); } } } LOG(INFO) << "Outlier/Inlier in pose estimating: " << cnt_outlier << "/" << features.size() - cnt_outlier; // Set pose and outlier current_frame_->SetPose(vertex_pose->estimate()); LOG(INFO) << "Current Pose = \n" << current_frame_->Pose().matrix(); for (auto &feat : features) { if (feat->is_outlier_) { feat->map_point_.reset(); feat->is_outlier_ = false; // maybe we can still use it in future } } return features.size() - cnt_outlier; } int Frontend::TrackLastFrame() { // use LK flow to estimate points in the right image std::vector kps_last, kps_current; for (auto &kp : last_frame_->features_left_) { if (kp->map_point_.lock()) { // use project point auto mp = kp->map_point_.lock(); auto px = camera_left_->world2pixel(mp->pos_, current_frame_->Pose()); kps_last.push_back(kp->position_.pt); kps_current.push_back(cv::Point2f(px[0], px[1])); } else { kps_last.push_back(kp->position_.pt); kps_current.push_back(kp->position_.pt); } } std::vector status; Mat error; cv::calcOpticalFlowPyrLK( last_frame_->left_img_, current_frame_->left_img_, kps_last, kps_current, status, error, cv::Size(11, 11), 3, cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); int num_good_pts = 0; for (size_t i = 0; i < status.size(); ++i) { if (status[i]) { cv::KeyPoint kp(kps_current[i], 7); Feature::Ptr feature(new Feature(current_frame_, kp)); feature->map_point_ = last_frame_->features_left_[i]->map_point_; current_frame_->features_left_.push_back(feature); num_good_pts++; } } LOG(INFO) << "Find " << num_good_pts << " in the last image."; return num_good_pts; } bool Frontend::StereoInit() { int num_features_left = DetectFeatures(); int num_coor_features = FindFeaturesInRight(); if (num_coor_features < num_features_init_) { return false; } bool build_map_success = BuildInitMap(); if (build_map_success) { status_ = FrontendStatus::TRACKING_GOOD; if (viewer_) { viewer_->AddCurrentFrame(current_frame_); viewer_->UpdateMap(); } return true; } return false; } int Frontend::DetectFeatures() { cv::Mat mask(current_frame_->left_img_.size(), CV_8UC1, 255); for (auto &feat : current_frame_->features_left_) { cv::rectangle(mask, feat->position_.pt - cv::Point2f(10, 10), feat->position_.pt + cv::Point2f(10, 10), 0, CV_FILLED); } std::vector keypoints; gftt_->detect(current_frame_->left_img_, keypoints, mask); int cnt_detected = 0; for (auto &kp : keypoints) { current_frame_->features_left_.push_back( Feature::Ptr(new Feature(current_frame_, kp))); cnt_detected++; } LOG(INFO) << "Detect " << cnt_detected << " new features"; return cnt_detected; } int Frontend::FindFeaturesInRight() { // use LK flow to estimate points in the right image std::vector kps_left, kps_right; for (auto &kp : current_frame_->features_left_) { kps_left.push_back(kp->position_.pt); auto mp = kp->map_point_.lock(); if (mp) { // use projected points as initial guess auto px = camera_right_->world2pixel(mp->pos_, current_frame_->Pose()); kps_right.push_back(cv::Point2f(px[0], px[1])); } else { // use same pixel in left iamge kps_right.push_back(kp->position_.pt); } } std::vector status; Mat error; cv::calcOpticalFlowPyrLK( current_frame_->left_img_, current_frame_->right_img_, kps_left, kps_right, status, error, cv::Size(11, 11), 3, cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); int num_good_pts = 0; for (size_t i = 0; i < status.size(); ++i) { if (status[i]) { cv::KeyPoint kp(kps_right[i], 7); Feature::Ptr feat(new Feature(current_frame_, kp)); feat->is_on_left_image_ = false; current_frame_->features_right_.push_back(feat); num_good_pts++; } else { current_frame_->features_right_.push_back(nullptr); } } LOG(INFO) << "Find " << num_good_pts << " in the right image."; return num_good_pts; } bool Frontend::BuildInitMap() { std::vector poses{camera_left_->pose(), camera_right_->pose()}; size_t cnt_init_landmarks = 0; for (size_t i = 0; i < current_frame_->features_left_.size(); ++i) { if (current_frame_->features_right_[i] == nullptr) continue; // create map point from triangulation std::vector points{ camera_left_->pixel2camera( Vec2(current_frame_->features_left_[i]->position_.pt.x, current_frame_->features_left_[i]->position_.pt.y)), camera_right_->pixel2camera( Vec2(current_frame_->features_right_[i]->position_.pt.x, current_frame_->features_right_[i]->position_.pt.y))}; Vec3 pworld = Vec3::Zero(); if (triangulation(poses, points, pworld) && pworld[2] > 0) { auto new_map_point = MapPoint::CreateNewMappoint(); new_map_point->SetPos(pworld); new_map_point->AddObservation(current_frame_->features_left_[i]); new_map_point->AddObservation(current_frame_->features_right_[i]); current_frame_->features_left_[i]->map_point_ = new_map_point; current_frame_->features_right_[i]->map_point_ = new_map_point; cnt_init_landmarks++; map_->InsertMapPoint(new_map_point); } } current_frame_->SetKeyFrame(); map_->InsertKeyFrame(current_frame_); backend_->UpdateMap(); LOG(INFO) << "Initial map created with " << cnt_init_landmarks << " map points"; return true; } bool Frontend::Reset() { LOG(INFO) << "Reset is not implemented. "; return true; } } // namespace myslam ================================================ FILE: code/ch12/myslam/src/map.cpp ================================================ /* * * Copyright (C) 2016 * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * */ #include "myslam/map.h" #include "myslam/feature.h" namespace myslam { void Map::InsertKeyFrame(Frame::Ptr frame) { current_frame_ = frame; if (keyframes_.find(frame->keyframe_id_) == keyframes_.end()) { keyframes_.insert(make_pair(frame->keyframe_id_, frame)); active_keyframes_.insert(make_pair(frame->keyframe_id_, frame)); } else { keyframes_[frame->keyframe_id_] = frame; active_keyframes_[frame->keyframe_id_] = frame; } if (active_keyframes_.size() > num_active_keyframes_) { RemoveOldKeyframe(); } } void Map::InsertMapPoint(MapPoint::Ptr map_point) { if (landmarks_.find(map_point->id_) == landmarks_.end()) { landmarks_.insert(make_pair(map_point->id_, map_point)); active_landmarks_.insert(make_pair(map_point->id_, map_point)); } else { landmarks_[map_point->id_] = map_point; active_landmarks_[map_point->id_] = map_point; } } void Map::RemoveOldKeyframe() { if (current_frame_ == nullptr) return; // 寻找与当前帧最近与最远的两个关键帧 double max_dis = 0, min_dis = 9999; double max_kf_id = 0, min_kf_id = 0; auto Twc = current_frame_->Pose().inverse(); for (auto& kf : active_keyframes_) { if (kf.second == current_frame_) continue; auto dis = (kf.second->Pose() * Twc).log().norm(); if (dis > max_dis) { max_dis = dis; max_kf_id = kf.first; } if (dis < min_dis) { min_dis = dis; min_kf_id = kf.first; } } const double min_dis_th = 0.2; // 最近阈值 Frame::Ptr frame_to_remove = nullptr; if (min_dis < min_dis_th) { // 如果存在很近的帧，优先删掉最近的 frame_to_remove = keyframes_.at(min_kf_id); } else { // 删掉最远的 frame_to_remove = keyframes_.at(max_kf_id); } LOG(INFO) << "remove keyframe " << frame_to_remove->keyframe_id_; // remove keyframe and landmark observation active_keyframes_.erase(frame_to_remove->keyframe_id_); for (auto feat : frame_to_remove->features_left_) { auto mp = feat->map_point_.lock(); if (mp) { mp->RemoveObservation(feat); } } for (auto feat : frame_to_remove->features_right_) { if (feat == nullptr) continue; auto mp = feat->map_point_.lock(); if (mp) { mp->RemoveObservation(feat); } } CleanMap(); } void Map::CleanMap() { int cnt_landmark_removed = 0; for (auto iter = active_landmarks_.begin(); iter != active_landmarks_.end();) { if (iter->second->observed_times_ == 0) { iter = active_landmarks_.erase(iter); cnt_landmark_removed++; } else { ++iter; } } LOG(INFO) << "Removed " << cnt_landmark_removed << " active landmarks"; } } // namespace myslam ================================================ FILE: code/ch12/myslam/src/mappoint.cpp ================================================ /* * * Copyright (C) 2016 * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * */ #include "myslam/mappoint.h" #include "myslam/feature.h" namespace myslam { MapPoint::MapPoint(long id, Vec3 position) : id_(id), pos_(position) {} MapPoint::Ptr MapPoint::CreateNewMappoint() { static long factory_id = 0; MapPoint::Ptr new_mappoint(new MapPoint); new_mappoint->id_ = factory_id++; return new_mappoint; } void MapPoint::RemoveObservation(std::shared_ptr feat) { std::unique_lock lck(data_mutex_); for (auto iter = observations_.begin(); iter != observations_.end(); iter++) { if (iter->lock() == feat) { observations_.erase(iter); feat->map_point_.reset(); observed_times_--; break; } } } } // namespace myslam ================================================ FILE: code/ch12/myslam/src/viewer.cpp ================================================ // // Created by gaoxiang on 19-5-4. // #include "myslam/viewer.h" #include "myslam/feature.h" #include "myslam/frame.h" #include #include namespace myslam { Viewer::Viewer() { viewer_thread_ = std::thread(std::bind(&Viewer::ThreadLoop, this)); } void Viewer::Close() { viewer_running_ = false; viewer_thread_.join(); } void Viewer::AddCurrentFrame(Frame::Ptr current_frame) { std::unique_lock lck(viewer_data_mutex_); current_frame_ = current_frame; } void Viewer::UpdateMap() { std::unique_lock lck(viewer_data_mutex_); assert(map_ != nullptr); active_keyframes_ = map_->GetActiveKeyFrames(); active_landmarks_ = map_->GetActiveMapPoints(); map_updated_ = true; } void Viewer::ThreadLoop() { pangolin::CreateWindowAndBind("MySLAM", 1024, 768); glEnable(GL_DEPTH_TEST); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); pangolin::OpenGlRenderState vis_camera( pangolin::ProjectionMatrix(1024, 768, 400, 400, 512, 384, 0.1, 1000), pangolin::ModelViewLookAt(0, -5, -10, 0, 0, 0, 0.0, -1.0, 0.0)); // Add named OpenGL viewport to window and provide 3D Handler pangolin::View& vis_display = pangolin::CreateDisplay() .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f) .SetHandler(new pangolin::Handler3D(vis_camera)); const float blue[3] = {0, 0, 1}; const float green[3] = {0, 1, 0}; while (!pangolin::ShouldQuit() && viewer_running_) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glClearColor(1.0f, 1.0f, 1.0f, 1.0f); vis_display.Activate(vis_camera); std::unique_lock lock(viewer_data_mutex_); if (current_frame_) { DrawFrame(current_frame_, green); FollowCurrentFrame(vis_camera); cv::Mat img = PlotFrameImage(); cv::imshow("image", img); cv::waitKey(1); } if (map_) { DrawMapPoints(); } pangolin::FinishFrame(); usleep(5000); } LOG(INFO) << "Stop viewer"; } cv::Mat Viewer::PlotFrameImage() { cv::Mat img_out; cv::cvtColor(current_frame_->left_img_, img_out, CV_GRAY2BGR); for (size_t i = 0; i < current_frame_->features_left_.size(); ++i) { if (current_frame_->features_left_[i]->map_point_.lock()) { auto feat = current_frame_->features_left_[i]; cv::circle(img_out, feat->position_.pt, 2, cv::Scalar(0, 250, 0), 2); } } return img_out; } void Viewer::FollowCurrentFrame(pangolin::OpenGlRenderState& vis_camera) { SE3 Twc = current_frame_->Pose().inverse(); pangolin::OpenGlMatrix m(Twc.matrix()); vis_camera.Follow(m, true); } void Viewer::DrawFrame(Frame::Ptr frame, const float* color) { SE3 Twc = frame->Pose().inverse(); const float sz = 1.0; const int line_width = 2.0; const float fx = 400; const float fy = 400; const float cx = 512; const float cy = 384; const float width = 1080; const float height = 768; glPushMatrix(); Sophus::Matrix4f m = Twc.matrix().template cast(); glMultMatrixf((GLfloat*)m.data()); if (color == nullptr) { glColor3f(1, 0, 0); } else glColor3f(color[0], color[1], color[2]); glLineWidth(line_width); glBegin(GL_LINES); glVertex3f(0, 0, 0); glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz); glVertex3f(0, 0, 0); glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz); glVertex3f(0, 0, 0); glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz); glVertex3f(0, 0, 0); glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz); glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz); glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz); glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz); glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz); glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz); glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz); glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz); glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz); glEnd(); glPopMatrix(); } void Viewer::DrawMapPoints() { const float red[3] = {1.0, 0, 0}; for (auto& kf : active_keyframes_) { DrawFrame(kf.second, red); } glPointSize(2); glBegin(GL_POINTS); for (auto& landmark : active_landmarks_) { auto pos = landmark.second->Pos(); glColor3f(red[0], red[1], red[2]); glVertex3d(pos[0], pos[1], pos[2]); } glEnd(); } } // namespace myslam ================================================ FILE: code/ch12/myslam/src/visual_odometry.cpp ================================================ // // Created by gaoxiang on 19-5-4. // #include "myslam/visual_odometry.h" #include #include "myslam/config.h" namespace myslam { VisualOdometry::VisualOdometry(std::string &config_path) : config_file_path_(config_path) {} bool VisualOdometry::Init() { // read from config file if (Config::SetParameterFile(config_file_path_) == false) { return false; } dataset_ = Dataset::Ptr(new Dataset(Config::Get("dataset_dir"))); CHECK_EQ(dataset_->Init(), true); // create components and links frontend_ = Frontend::Ptr(new Frontend); backend_ = Backend::Ptr(new Backend); map_ = Map::Ptr(new Map); viewer_ = Viewer::Ptr(new Viewer); frontend_->SetBackend(backend_); frontend_->SetMap(map_); frontend_->SetViewer(viewer_); frontend_->SetCameras(dataset_->GetCamera(0), dataset_->GetCamera(1)); backend_->SetMap(map_); backend_->SetCameras(dataset_->GetCamera(0), dataset_->GetCamera(1)); viewer_->SetMap(map_); return true; } void VisualOdometry::Run() { while (1) { LOG(INFO) << "VO is running"; if (Step() == false) { break; } } backend_->Stop(); viewer_->Close(); LOG(INFO) << "VO exit"; } bool VisualOdometry::Step() { Frame::Ptr new_frame = dataset_->NextFrame(); if (new_frame == nullptr) return false; auto t1 = std::chrono::steady_clock::now(); bool success = frontend_->AddFrame(new_frame); auto t2 = std::chrono::steady_clock::now(); auto time_used = std::chrono::duration_cast>(t2 - t1); LOG(INFO) << "VO cost time: " << time_used.count() << " seconds."; return success; } } // namespace myslam ================================================ FILE: docs/_sidebar.md ================================================ - [1.一幅图像的诞生：相机投影及相机畸变](chapter1/chapter1) - [2.差异与统一：坐标系变换与外参标定](chapter2/chapter2) - [3.描述状态不简单：三维空间刚体运动](chapter3/chapter3) - [4.也见森林：视觉SLAM简介](chapter4/chapter4) - [5.以不变应万变：前端-视觉里程计之特征点](chapter5/chapter5) - [6.换一个视角看世界：前端-视觉里程计之对极几何](chapter6/chapter6) - [7.积硅步以至千里：前端-视觉里程计之相对位姿估计](chapter7/chapter7) - [8.在不确定中寻找确定：后端之卡尔曼滤波](chapter8/chapter8) - [9.每次更好，就是最好：后端之非线性优化](chapter9/chapter9) - [10.又回到曾经的地点：回环检测](chapter10/chapter10) - [11.终识庐山真面目：建图](chapter11/chapter11) - [12.实践是检验真理的唯一标准：设计VSLAM系统](chapter12/chapter12) - [思考题参考答案](reference_answers_for_each_chapter/reference_answers_for_each_chapter) ================================================ FILE: docs/chapter1/chapter1.md ================================================ # **1.一幅图像的诞生：相机模型及坐标系** 一个能思考的人，才真是一个力量无边的人。——巴尔扎克 --- > **本章主要内容：** \ > 本教程结构 \ > 1.相机模型 \ > 2.相机成像的几个坐标系 \ > 3.相机标定 本节主要介绍相机成像过程及相机模型， 相机模型是描述相机如何将三维世界投影到二维图像平面的数学模型，一般包括：\ **1.投影模型：** 描述从三维世界坐标到图像坐标的映射，常见的模型有针孔相机模型。 \ **2.畸变模型：** 描述实际物理相机镜头因为光学误差或设计缺陷所产生的畸变 ，常见的包括径切变rad-tan模型或者KB模型。
相机模型 = 投影模型 (Projection Model) + 畸变模型 (Distortion Model)
在常用的相机标定工具如kalibr中，用--models参数来指定相机模型，如： \ 针孔径切畸变相机：pinhole-radtan \ 针孔鱼眼畸变相机： pinhole-equi \ 其中pinhole指投影模型为针孔，radtan与equi分别是普通相机和鱼眼相机的畸变模型。 ## **1.1 针孔投影模型** 针孔相机模型是目前大多数相机的成像模型，其成像原理为小孔成像，回顾一下我们小学二年级学过的内容，光线在同一介质中按直线传播，在穿过小孔之后，会在另一面形成倒立缩小的实像。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986390545-0b0cb59b-73de-4d12-bc91-bfe139dfd562.png#averageHue=%23e8e7e6&from=url&height=333&id=nimTZ&originHeight=420&originWidth=827&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=656)
如果不借助其他东西，这个成像过程会遵循如下原理：小孔越小，成像越清晰，但因为进光更少，所以图案会越暗。 \ 如下图，小孔直径从2mm变到0.35mm过程，**图像越来越清晰，但也越来越暗。**
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986390812-026c7e81-0397-4ec3-90b3-383c00d74fb1.png#averageHue=%236f6f6f&from=url&height=377&id=blUR7&originHeight=432&originWidth=584&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=508.9840087890625)
但我们总是既要又要，想要照片既清晰，同时又要有足够的亮度。从物理上来说，就是要让足够的光线进来，同时又能把光汇聚在一起。为了达到这个效果，透镜闪亮登场：
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986391243-dfc73522-763c-4381-8406-1e4729d0fbee.png#averageHue=%23f4f3f3&from=url&height=292&id=UkgnL&originHeight=362&originWidth=661&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=533.9866333007812) 透镜是现代相机的一大杀手锏，解决了成像的清晰度与亮度问题，为了达到更好的成像效果，现代相机镜头都是由多种透镜组合而成： ![](https://cdn.nlark.com/yuque/0/2026/jpeg/1782698/1771601881457-ff3ad660-00ac-4bb5-8e87-21d11aaae275.jpeg) 简单来说，**相机 = 镜头 （Lens） + 图像传感器 （Image Sensor）** ![](https://cdn.nlark.com/yuque/0/2026/png/1782698/1771602696099-5fc0e13d-7738-4d07-9a34-745f6a0859af.png) ## **1.2 畸变模型** 光线在穿过众多镜片到达底片上时，与理想的小孔成像的光路会有所差异，比如下方的鱼眼镜头成像过程，为了模拟镜头对光线的折射效果，于是出现了畸变模型。\ ![](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687660584100-75eae03e-ee8e-4685-8c00-a43d957c77d5.png) 最常用的畸变模型有Radtan径切畸变模型(对于标准镜头)及Kannala-Brandt模型（对于广角及鱼眼镜头） ### **1.2.1径向畸变** 径向畸变的原因与相机镜头的设计和光学特性密切相关。特别是对于广角镜头，视角较宽，光线进入镜头时，靠近边缘的光线会受到镜头元素的非均匀折射，导致图像畸变。\ **径向畸变的类型：** + **桶形畸变（Barrel Distortion）**：这种畸变通常发生在广角镜头和超广角镜头中，图像中心的物体保持正常，而越靠近边缘的物体越容易被拉伸，造成图像边缘像一个桶一样膨胀。 + **表现**：图像的边缘部分会被拉向外部，像桶的形状。 + **枕形畸变（Pincushion Distortion）**：这种畸变常见于长焦镜头，图像的边缘部分被压缩，导致边缘向内弯曲，像枕头一样。 + **表现**：图像的边缘部分会被压向中心，形成枕形的效果。 ![](https://cdn.nlark.com/yuque/0/2026/png/1782698/1771653053445-2fb92245-a3b8-4ab5-9fa4-8ccf8f6b78ae.png) 对于径向畸变，常用如下公式进行修正
$\begin{array}{l} x_{\text {distortion }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \\ y_{\text {distortion }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \end{array} \\ \text { 其中: } r^{2}=x^{2}+y^{2}$
k1,k2,k3称为径向畸变系数。 ### **1.2.2切向畸变** 切向畸变来自于整个摄像机的组装过程。由于透镜制造上的缺陷使得透镜本身与图像平面不平行而产生的，如下图所示。\ ![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986394315-ff2ca78a-5437-4bc3-a92d-f37234818c77.png) 切向图像畸变使用如下公式进行修正
$\begin{aligned} x_{\text {distortion }} & =x+\left[2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right)\right] \\ y_{\text {distortion }} & =y+\left[p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y\right] \end{aligned} \\ 其中: r^{2}=x^{2}+y^{2}$
其中 p1,p2为切向畸变系数。
将径向畸变与切向畸变校正，结合在一起，便是常见的畸变过程。
$\begin{array}{l} x_{\text {distortion }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+\left[2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right)\right] \\ y_{\text {distortion }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+\left[p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y\right] \end{array} \\ 其中: r^{2}=x^{2}+y^{2}$
等式右边的(x, y)是理想小孔成像的照片上像素点坐标，没有畸变，将其带入等式右边，经过径向和切向变换后，得到左边的添加畸变后的实际点坐标(Xdistortion, Ydistortion)，就是我们实际获得的照片，如果我们将这个映射取逆，就可以对存在畸变的实际图像，进行去畸变，得到去畸变后的图像： ![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986394648-56f1f7d0-7375-4bd0-a09a-066b219b5fdd.png) ### **1.2.3Kannala-Brandt模型** Kannala-Brandt模型（简称KB模型）是处理广角和鱼眼相机时最主流的相机模型之一。它在2006年由Juho Kannala和Sami S. Brandt提出，其核心思想是使用一个高阶多项式来统一描述各种镜头的投影和畸变，尤其擅长处理超过180°视场的极端广角场景。简单来说，KB模型对入射光线入射角与投影位置进行近似模拟，最简单的就是认为投影位置与入射角θ成线性正比关系，即 $$ r_{d} = f \theta $$ 上面的公式就是等距模型，KB模型中一共有四种假设模型，通过这四个模型，发现投影位置是θ的奇函数，将这些式子按泰勒级数展开，畸变位置可以用θ的奇次多项式表示，即 $$ \theta_{d}=k_{0} \theta+k_{1} \theta^{3}+k_{2} \theta^{5}+\cdots $$ 取前5项，给出了足够的自由度来很好的近似各种投影模型。 $$ \theta_{d}=k_{0} \theta+k_{1} \theta^{3}+k_{2} \theta^{5}+k_{3} \theta^{7}+k_{4} \theta^{9} $$ 上式是根据四种鱼眼相机投影模型得出的一种通用鱼眼相机多项式模型。OpenCV中鱼眼相机标定方法就是用的这种KB模型。其中一般会假设k0为1，然后标定得到k1,k2,k3,k4四个畸变系数。 ## **1.3 不同相机及其适用的畸变模型** 首先要注意的是，不同相机模型，畸变的大小是不同的，简单来说，FOV(视场角)越大的相机，畸变程度就越大。 ![](https://cdn.nlark.com/yuque/0/2026/png/1782698/1771652733011-ed7a01bd-cce4-4077-bdd7-dae2b82a73c8.png) 在视觉SLAM中，不同FOV镜头适用的畸变模型如下： | 镜头类型 | FoV范围 | 畸变模型 | 特点 | | --- | --- | --- | --- | | 长焦 | 20°~40° | radtan | 大多数长焦镜头会表现出一些**轻微的桶形畸变**，但一般不显著。由于其较长的焦距，图像的边缘处畸变通常较不明显 | | 标准 | 40°~60° | radtan | 标准镜头通常表现出非常少的畸变，尤其是在相机的中心区域。大多数标准镜头的畸变在图像中心非常轻微，但在图像的边缘可能出现**轻微的桶形畸变** | | 广角 | 60°~90° | 高阶radtan | 广角镜头有较大的视场角，因此畸变较为明显，尤其是在图像的边缘区域。广角镜头通常会出现**桶形畸变**（图像中心物体正常，边缘物体被拉伸或扭曲），并且会出现更多的**几何畸变**。 | | 超广角 | 90°~180° | KB模型 | 超广角镜头有非常大的视场角，因此畸变会更加严重，尤其是在画面边缘。超广角镜头经常出现**明显的桶形畸变**，而且在一些极端情况下，还会出现**鱼眼效应**（图像中心正常，边缘极度弯曲） | | 鱼眼 | ≥180° | KB模型 | 鱼眼镜头的畸变非常强烈，几乎是**极端的桶形畸变**，并且呈现出明显的“球形”畸变，导致图像中心正常而边缘区域严重拉伸。鱼眼镜头的视场角可以达到180度，甚至更广，所有图像都被极度弯曲。 | ## **1.4 感光元件** 现代相机都是数码相机，使用电子感光元件进行图像记录。感光元件一般为CCD或者CMOS，其原理是将光转化为模拟电信号来记录。 \ ![](https://cdn.nlark.com/yuque/0/2026/png/1782698/1771657003181-f9171815-f3c3-43a5-addc-b79c506c4631.png) > 电子感光元件也叫图像传感器（Image Sensor），分为两种：一种是广泛使用的 CCD(电荷耦合)元件，另一种是 CMOS(互补金属氧化物半导体)器件。其产生的模拟信号，首先经过模拟信号放大器进行信号放大，进而经过数模转换电路（DAC）变为数字图像，数字图像再经过 ISP（Image Signal Processor）图像处理器进行数字图像处理，最后数字图像经过压缩编码算法，存储到 SD 卡中成为一个照片文件。 > > 1. CCD > > CCD 全称 Charge Coupled Device，它使用一种高感光度的半导体材料制成，由许多感光单位组成，通常以百万像素为单位。当 CCD 表面受到光线照射时，每个感光单位会将电荷反映在组件上，即把光转换为电荷，所有的感光单位所产生的信号加在一起，就构成了一幅完整的画面。 > > 2. CMOS > > CMOS 全称 Complementary Metal-Oxide Semiconductor，它主要是利用硅和锗这两种元素所做成的半导体，使其在 CMOS 上共存着 N 极和 P 极的半导体，这两个互补效应所产生的电流即可被处理芯片记录为影像。 > > 两者最主要的区别在于：CCD 传感器的图像质量优于 CMOS 传感器，而 CMOS 传感器在成像速度、功耗、价格等方面优于 CCD 传感器。 > 传感器生成的图像，是对连续世界的离散采样，即一个一个像素，我们所说的图像，也是由这一个一个像素构成，在计算机下面来看，就是一个二维数组。下图中的uv坐标系就是像素坐标系。 ![](https://cdn.nlark.com/yuque/0/2026/png/1782698/1771657195302-da4a4bd4-e88d-45e4-89bb-45b1c9889eae.png) ## **2.2 相机成像的几个坐标系** 要把相机拍摄的照片与实际物体关联起来，就要建立三维世界到二维图像平面的映射关系，这个过程主要通过世界坐标系，相机坐标系，图像坐标系与像素坐标系这几个坐标系之间的转换来实现。 ### **2.2.1成像坐标系之间的关系** 世界坐标系下物体的光线（世界坐标系），通过透镜（相机坐标系），投射到感光原件上（图像坐标系），最后计算机在像素坐标系（离散化过程）上做处理。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986391552-09e1f357-0aaf-491b-9360-9d48ccf83700.png#averageHue=%23c3ad94&from=url&height=289&id=j2KM4&originHeight=427&originWidth=1000&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=677.2740478515625)
**世界坐标系**：用于表示空间物体的绝对坐标，使用（Xw,Yw,Zw）表示，世界坐标系可通过旋转和平移得到相机坐标系。
**相机坐标系**：以相机的光心为坐标系原点，Xc.Yc轴平行于图像坐标系的x,y轴，相机的光轴为Zc轴，坐标系满足右手法则，相机的光心可理解为相机透镜的几何中心。
**图像物理坐标系**：坐标原点在CCD图像平面的中心x,y轴分别平行于图像像素坐标系的(u,v)轴，坐标用(x,y)表示。
**图像像素坐标系**：表示三维空间物体在图像平面上的投影，**像素是离散化的**，其坐标原点在CCD图像平面的左上角，u轴平行于CCD平面水平向右，v轴垂直于u轴向下，坐标使用(u,v)来表示。图像宽度W，高度H。 ### **2.2.2 坐标计算** ![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986391785-d7c5d2c5-1a31-4690-839d-ea1187575387.png#averageHue=%23fcfbfa&from=url&id=qBc3w&originHeight=504&originWidth=883&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=)
三维坐标投影到成像平面的坐标（**完成三维到二维点的映射**），可以通过相似三角形得出，对于相机坐标系下的P(X,Y,Z)，成像坐标为：
$\frac{f}{Z}=-\frac{X^{\prime}}{X}=-\frac{Y^{\prime}}{Y}$
为了方便运算，取对称的镜像进行计算，效果等价。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986392278-85592406-7069-4440-bbad-03d8252d1c05.png#averageHue=%23fafafa&from=url&id=vxoSW&originHeight=346&originWidth=997&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=)
$\frac{f}{Z}=\frac{X^{\prime}}{X}=\frac{Y^{\prime}}{Y}$
可得：
$X^{\prime}=f \frac{X}{Z}$
$Y^{\prime}=f \frac{Y}{Z}$
计算机中的图像，是一个个像素构成，且其并不是图像中心为坐标原点，而是一般把左上角规定为坐标原点，要在计算机中处理图像信息，需要在得到成像平面上的坐标后，把成像坐标，转为像素坐标。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986392829-dc8f8311-ec2d-4920-9494-f7a42b1aa0ca.png#averageHue=%23f4f3f3&from=url&height=347&id=cUKgL&originHeight=530&originWidth=558&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=364.9952087402344)
其中O1是投影后的坐标系原点，位于图像中心，而在计算机图像处理库中（如OpenCV）,则是定义的左上角O0为图像坐标系原点，横坐标轴为u轴，向右，纵坐标轴为v轴，向下。
相机内感光原件（如cmos）是一个一个小格子拼接而成的，可以认为是离散的，这个小格子可能不是正方形，要将投影坐标P'转化为像素坐标，需要经历如下过程：
计算P'到图像中心的像素距离
$u^{\prime}=\frac{X^{\prime}}{\alpha_{x}}=\frac{f}{\alpha_{x}} \frac{X}{Z}=f_{x} \frac{X}{Z}$
$v^{\prime}=\frac{Y^{\prime}}{\alpha_{y}}=\frac{f}{\alpha_{y}} \frac{Y}{Z}=f_{y} \frac{Y}{Z}$
其中f为成像焦距，αx与αy为u,v方向像素的长度。
最后转为O0下的坐标
实现摄像机下三维世界的点到像素平面二维图像平面的点的映射，f为单位米转化为像素的数量，**非线性变换**如下
$u=u^{\prime}+c_{x}=f_{x} \frac{X}{Z}+c_{x}$
$v=v^{\prime}+c_{y}=f_{y} \frac{Y}{Z}+c_{y}$
引入齐次坐标，改为线性变换，以方便运算
$\left(\begin{array}{ccc} u \\ v \\ 1 \end{array}\right)=\frac{1}{Z}\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{c} X \\ Y \\ Z \end{array}\right)=\frac{1}{Z} K P \\$
其中
$K=\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)$
就是通常所说的相机内参矩阵
通过内参矩阵K，就可以将相机坐标系的下三维的坐标点转换为图像上的二维像素坐标。而对于世界坐标系上的坐标，转换到相机坐标系中则涉及的是不同坐标系中坐标转换，这个会在第二章中进行讲解。 ## **本章小结** 本节以针孔相机模型作为基本模型，简要介绍了相机成像原理，相机模型中的几个坐标系及相机的畸变及相机标定的方法。最后鱼眼相机模型作为扩展介绍。
通过相机标定，可以获得相机内参及畸变系数，这解决了相机坐标系，图像坐标系，像素坐标系之间的转换过程，属于投影过程。而世界坐标系到相机坐标系之间的转换，则涉及到不同坐标系之间的空间转换，这部分涉及的参数为外参，这部分内容在下节进行讲解。 ## 本章思考 1.叙述相机内参的物理意义。如果一幅图像长宽缩小变为原来的一半而其他不变，那么它的内参将如何变化？
2.调研全局快门(global shutter)相机和卷帘快门(rolling shutter)相机的异同。它们在SLAM中有何优缺点？ ## 扩展 **a.透镜的聚焦和失焦** >关于透镜，有两个概念：聚焦与失焦 \ 聚焦：从物体不同部分射出的光线，通过镜头之后，聚焦在底片的一个点上，使影像具有清晰的轮廓与真实的质感，这个过程称为聚焦。 \ 失焦：即接收的点的信息未聚焦到一起会导致成像模糊。 注意：物体“聚焦”有特定距离（景深），在景深内可清晰成像，景深外成像模糊。 \ 加入透镜之后，成像规律会有一点变化，此时当物体离透镜不同距离时，会形成不同的像。当物体处于凸透镜的 2 倍焦距之外，会形成倒立的、缩小的实像。一倍焦距到二倍焦距之间，则会形成倒立的、放大的实像。成像物体则在一倍焦距内，成正立的、放大的虚像。 一般地，相机成像时，**物体在透镜的二倍焦距之外**。而对于投影仪，则会把成像物体放在一倍焦距到二倍焦距之间。对于放大镜，成像物体则在一倍焦距内。 ![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1693316432667-185ba964-b59e-4982-826f-d54b474d156b.png) **b.鱼眼相机模型** \ 鱼眼镜头一般是由十几个不同的透镜组合而成的，在成像的过程中，入射光线经过不同程度的折射，投影到尺寸有限的成像平面上，使得鱼眼镜头与普通镜头相比起来拥有了更大的视野范围。下图表示出了鱼眼相机的一般组成结构。最前面的两个镜头发生折射，使入射角减小，其余的镜头相当于一个成像镜头，这种多元件的构造结构使对鱼眼相机的折射关系的分析变得相当复杂。
![](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687660584100-75eae03e-ee8e-4685-8c00-a43d957c77d5.png#averageHue=%23f7f7f6&clientId=u066aa5a8-e0e4-4&from=paste&id=oC0TR&originHeight=230&originWidth=537&originalType=url&ratio=1.1699999570846558&rotation=0&showTitle=false&status=done&style=none&taskId=u7bdac733-b9b6-4cd5-9f51-dddefe3dd74&title=) **b.鱼眼相机成像模型** \ 研究表明鱼眼相机成像时遵循的模型可以近似为单位球面投影模型。可以将鱼眼相机的成像过程分解成两步：第一步，三维空间点线性地投影到一个球面上，它是一个虚拟的单位球面，它的球心与相机坐标系的原点重合；第二步，单位球面上的点投影到图像平面上，这个过程是非线性的。下图表示出了鱼眼相机的成像过程。
![](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687660618939-3f5d7d9f-a374-4645-9821-9964e3251aba.png#averageHue=%23f7f7f7&clientId=u066aa5a8-e0e4-4&from=paste&id=OkCWP&originHeight=294&originWidth=233&originalType=url&ratio=1.1699999570846558&rotation=0&showTitle=false&status=done&style=none&taskId=u84b7e864-b123-4e4d-8c2e-6a0e5bfb9d1&title=)
我们知道，普通相机成像遵循的是针孔相机模型，在成像过程中实际场景中的直线仍被投影为图像平面上的直线。但是鱼眼相机如果按照针孔相机模型成像的话，投影图像会变得非常大，当相机视场角达到180°时，图像甚至会变为无穷大。所以，鱼眼相机的投影模型为了将尽可能大的场景投影到有限的图像平面内，允许了相机畸变的存在。并且由于鱼眼相机的径向畸变非常严重，所以鱼眼相机主要的是考虑径向畸变，而忽略其余类型的畸变。 ### **1* 鱼眼相机投影函数** 为了将尽可能大的场景投影到有限的图像平面内，鱼眼相机会按照一定的投影函数来设计。根据投影函数的不同，鱼眼相机的设计模型大致能被分为四种：等距投影模型、等立体角投影模型、正交投影模型和体视投影模型。下面的四种鱼眼相机的投影模型反映出了空间中的一点P是如何投影到球面上，然后到图像平面上成像的。
1、等距投影模型
![](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687660957922-a6ebbb79-3eb5-437d-a05e-7cb70d4a7c4d.png#averageHue=%23f8f8f8&clientId=u066aa5a8-e0e4-4&from=paste&height=242&id=bpNAU&originHeight=242&originWidth=225&originalType=url&ratio=1.1699999570846558&rotation=0&showTitle=false&status=done&style=none&taskId=ubdd671d3-fca4-4ada-bdd9-868907e2709&title=&width=225)
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687660983508-d1ade030-a453-4955-829c-53f445b881af.png#averageHue=%23faf8f7&clientId=u066aa5a8-e0e4-4&from=paste&height=32&id=MGzoP&originHeight=38&originWidth=89&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=1409&status=done&style=none&taskId=uefcc7702-ec3e-4751-beea-4495cc14922&title=&width=76.06837885854758)
上述式子中，rd表示鱼眼图像中的点到畸变中心的距离，是鱼眼相机的焦距，是入射光线与鱼眼相机光轴之间的夹角，即入射角。
2、等立体角投影模型
![](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687661008609-8b68d151-b609-4e05-acd0-81e5bad7b128.png#averageHue=%23f7f7f7&clientId=u066aa5a8-e0e4-4&from=paste&height=242&id=KH8Te&originHeight=242&originWidth=225&originalType=url&ratio=1.1699999570846558&rotation=0&showTitle=false&status=done&style=none&taskId=uc456cad4-ec70-40fa-b9cd-d5a4beeac09&title=&width=225)
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687661038593-b1e1d476-df94-4e89-8c07-052c59ed5e77.png#averageHue=%23faf8f7&clientId=u066aa5a8-e0e4-4&from=paste&height=48&id=MEUqn&originHeight=56&originWidth=160&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=3382&status=done&style=none&taskId=ua001d2e3-8ebe-4714-9b65-bb400ca71e4&title=&width=136.75214176817542)
3、正交投影模型
![](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687661076454-c7088e4b-ed7b-4145-9802-8fb0a91cc83b.png#averageHue=%23f8f8f7&clientId=u066aa5a8-e0e4-4&from=paste&height=242&id=Dkw6g&originHeight=242&originWidth=228&originalType=url&ratio=1.1699999570846558&rotation=0&showTitle=false&status=done&style=none&taskId=ue59659ae-63ca-4f9e-be3f-b68fd9a79fb&title=&width=228)
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687661087815-acc9a0ac-6908-45bc-a370-93fffbb79a7d.png#averageHue=%23f8f6f3&clientId=u066aa5a8-e0e4-4&from=paste&height=30&id=bfiqC&originHeight=35&originWidth=132&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=2623&status=done&style=none&taskId=uda9dde4c-f949-4d70-a464-ec3d612d839&title=&width=112.82051695874472)
4、体视投影模型
![](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687661111947-799bd123-f1a7-474b-9deb-4d495f9bec21.png#averageHue=%23f7f7f7&clientId=u066aa5a8-e0e4-4&from=paste&height=242&id=XjnKO&originHeight=242&originWidth=228&originalType=url&ratio=1.1699999570846558&rotation=0&showTitle=false&status=done&style=none&taskId=ubf455b81-c61d-4b66-932c-fdf87de26fb&title=&width=228)
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687661122635-f556dd0c-5302-4b9a-965b-74a0de31c375.png#averageHue=%23faf9f8&clientId=u066aa5a8-e0e4-4&from=paste&height=57&id=NHBuD&originHeight=67&originWidth=157&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=3493&status=done&style=none&taskId=uf357e6d4-10fc-4c49-8509-5abc178dc6f&title=&width=134.18803911002212) ## 附录 ## **1.1相机内参标定简介** 到目前为止，相机成像有两大转换过程，相机投影及畸变消除，主要由：
$K=\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)$
内参矩阵以及畸变参数：Distortioncoefficients=(k1，k2，k3, p1，p2)，这两种参数来决定。而求出这两类参数的过程，就是相机标定，目前相机常用的方法是借助棋盘格标定板，在相机前面拿着标定板上下左右前后移动，然后借助标定算法来求出以上参数，常用的标定算法有张正友标定法。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986395167-73c8a8c5-1143-47b8-8ab1-bb2c1492f913.png#averageHue=%23b9bab2&from=url&height=417&id=JEqcg&originHeight=587&originWidth=807&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=573.2617797851562) ## 1.2 相机内参标定工具 ### **1.2.1 OpenCV** OpenCV是一款广泛使用的计算机视觉库，其中包含相机内参标定的相关函数。在OpenCV中，使用calibrateCamera函数进行相机内参标定，该函数使用棋盘格等标定板，通过对标定板拍摄的多幅图像进行处理，得出相机的内参参数。OpenCV还提供了相关的可视化工具，如drawChessboardCorners函数，用于显示标定板的角点，以及projectPoints函数，用于将3D点投影到2D图像平面上。 1. 循环读取图片 2. 使用findChessboardCorners函数检测角点（需提前输入角点数） 3. 使用find4QuadCornerSubpix函数对角点进行亚像素精确化 4. 可用drawChessboardCorners将角点显示 5. 根据角点数和尺寸创建一个理想的棋盘格（用point向量存储所有理论上的角点坐标） 6. 通过calibrateCamera函数由理想坐标和实际图像坐标进行标定，可得到标定结果 7. 由projectPoints函数计算反向投影误差 ![](https://cdn.nlark.com/yuque/0/2023/webp/36012760/1687663768616-862df2ce-85c5-46b6-9695-a0c9ea16bef7.webp#averageHue=%23303030&clientId=u066aa5a8-e0e4-4&from=paste&height=434&id=SJuki&originHeight=882&originWidth=1184&originalType=url&ratio=1.1699999570846558&rotation=0&showTitle=false&status=done&style=none&taskId=u1c996264-c94e-448c-8285-d36a0ac7531&title=&width=582) ### **1.2.2 MATLAB** MATLAB也提供了相机内参标定的工具箱，包括Camera Calibration Toolbox和Image Processing Toolbox等。Camera Calibration Toolbox使用标定板对相机进行标定，并提供了可视化工具，如ShowExtrinsics函数，用于显示相机的外参参数。Image Processing Toolbox提供了更加高级的算法，如多目相机标定，以支持更加复杂的应用场景。
简单过程如下： 1. 应用程序中找到Camera Calibration 2. 添加标定板拍摄图片（按Ctrl可一次添加多张） 3. 输入棋盘格每格的尺寸大小 4. 显示已检测出的棋盘格，点击Calibration，开始标定。 5. 得到标定结果（平均误差小于0.5即可认为结果可靠） 6. 可查看标定结果和程序 ![](https://cdn.nlark.com/yuque/0/2023/webp/36012760/1687663682845-d22fc5a1-99a8-4335-b590-2996e28fda18.webp#averageHue=%23aaa89f&clientId=u066aa5a8-e0e4-4&from=paste&height=356&id=RP7n9&originHeight=679&originWidth=1184&originalType=url&ratio=1.1699999570846558&rotation=0&showTitle=false&status=done&style=none&taskId=uef08f4ec-b8ae-43a2-8f43-558c25f8784&title=&width=621.2625122070312) ### **1.2.3 ROS** ROS（Robot Operating System）是一种常用的机器人操作系统，其中包含相机内参标定的相关包，如camera_calibration。该包通过对标定板拍摄的多幅图像进行处理，计算出相机的内参参数，并自动保存标定结果。此外，ROS还提供了一系列可视化工具，如image_view，用于显示相机的图像和标定结果。 ## **参考** [1.图像处理——4个坐标系及相关转换图像像素坐标系 图像物理坐标系 相机坐标系 世界坐标系](https://blog.csdn.net/MengYa_Dream/article/details/120233806)
[2.相机畸变模型及去畸变计算](https://blog.csdn.net/TimeRiverForever/article/details/117283430)
[3.相机透镜及畸变](https://robot.czxy.com/docs/camera/chapter01/05-distortions/) [4. 最详细、最完整的相机标定讲解_51CTO博客_相机标定](https://blog.51cto.com/u_15242250/2870251) [5.鱼眼相机成像模型_sylvester0510的博客-CSDN博客](https://blog.csdn.net/u010128736/article/details/52864024) [6.The Camera Basics for Visual SLAM](https://www.kudan.io/blog/camera-basics-visual-slam/) ================================================ FILE: docs/chapter10/chapter10.md ================================================ # **10.又回到曾经的地点：回环检测** 学习这件事不在乎有没有人教你，最重要的是在于你自己有没有觉悟和恒心。——法布尔 --- > **本章主要内容** \ > 1.回环检测的作用 \ > 2.视觉常用回环检测方法 \ > 3.使用词袋模型进行回环检测 前面几个章节介绍了VSLAM系统中前端和后端，这里介绍下一个重要模块-回环检测。回环检测作为VSLAM中相对独立又重要的一个模块，在消除累积误差，重定位等方面有着重要作用，更有甚者，把系统是否有重定位模块，来作为判断一个系统是SLAM还是里程计的依据，那么接下来我们看下到底什么是回环模块，其为什么会有这么重要的作用。 ## 10.1 回环检测 回环检测顾名思义，就是检测当前是否有回环。如下面所示，在场景A中，我们从1点出发，沿着某条路线走，如果没有碰到之前的场景，我们的路线一直处于开环状态。当到达地点3时，属于之前走过的地方，路线闭合，形成闭环，这个闭合过程称为发生了回环。如果我们继续沿着之前的路线走（下图中白色虚线框内路径），则会不断发生新的回环，继续产生多个回环点。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686450269185-5402bb19-b1e3-41f2-a6c4-ba6629da887e.png#averageHue=%23515642&clientId=ud4f25423-17a1-4&from=paste&height=393&id=uf9786643&originHeight=674&originWidth=987&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=1060710&status=done&style=none&taskId=uf9f6454d-c770-4257-8453-1e0e83f2b85&title=&width=576)
为什么会有回环检测呢，或者说回环检测的作用是什么。这一切都和里程计的累计误差有关系。在上述场景，我们理想中的轨迹是这样的：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686451887713-d9cdfd3b-e916-4bf4-b836-38867a008a0e.png#averageHue=%234f5541&clientId=ud4f25423-17a1-4&from=paste&height=312&id=u75b944cb&originHeight=498&originWidth=713&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=578752&status=done&style=none&taskId=u864d934e-804f-4b99-ad27-80a867d0c3a&title=&width=446.4000244140625)
由于里程计的累计误差，走的越远越不准，最后实际估计的路线可能是下图这样，可以看到，从左侧开始，轨迹就逐渐开始偏掉了：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686452217459-bbb476c9-e224-477a-a45d-a9ef437572c9.png#averageHue=%234f5440&clientId=ud4f25423-17a1-4&from=paste&height=308&id=uc25a5108&originHeight=498&originWidth=708&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=581055&status=done&style=none&taskId=u76c20418-775d-4a30-bf81-41d5d0670eb&title=&width=438.4000244140625)
导致最后我们回到交叉路口时，人是回来了，估计的轨迹没有回来（下图左），整个轨迹与实际相比，出现了很大的偏差。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686453615749-821054ef-eef7-40d9-93a0-4bcf69f2c664.png#averageHue=%23535743&clientId=ud4f25423-17a1-4&from=paste&height=401&id=ue02c3143&originHeight=501&originWidth=1278&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=520958&status=done&style=none&taskId=u77cb3306-3bb9-4544-b837-ce951a76b90&title=&width=1022.4)
如果能通过算法检测到当前发生了回环，即上图中的两个绿点，其实是同一个地方，就可以通过匹配算法计算两个绿点之间的相对位置关系（黄线），通过这个相对位置关系，可以得到3点真实的位姿。把这个相对约束加到优化中，就可以把轨迹拉到正确的位置上，得到一个全局一致的姿态估计。
所以我们说，回环检测是消除里程计累计误差的好帮手。想在长时间，大范围的姿态估计中获得好的结果，回环检测是不可或缺的。更极端一点的是，我们会把只有前端和局部优化的算法叫做里程计，而有回环检测和全局优化的算法，称为SLAM算法。
另外也可以发现，回环检测算法也可以用在重定位之中。检测到回环后，可以通过匹配算法计算回环位置与当前位置之间的相对位置关系，然后得到当前的真实位姿。因此在定位阶段，回环检测的原理也能发挥重要的作用，回环检测算法真是一个应用很广泛的算法。 ## 10.2 回环检测的方法 现在回过头来，如何做回环检测呢？对于视觉SLAM，最简单的方法是对任意两幅图像都做一遍特征匹配，根据正确匹配的数量确定哪两幅图像存在关联。这种方法朴素但是有效，缺点是任意两幅图像做特征匹配，计算量实在太大，因此不实用。退而求其次的话，随机抽取历史数据进行回环检测，比如在帧中随机抽5帧与当前帧比较。时间效率提高很多，但是抽到回环几率不高，也就是检测效率不高。
另外也有如下的一些可能方式： - **1.基于里程计几何关系的方法：** 1.大概思路是当我们发现当前相机运动到之前的某个位置附近时，检测他们是否存在回环
缺点：由于累积误差，很难确定运动到了之前某个位置 - **2.基于外观的方法** 1.仅仅依靠两幅图像的相似性确定回环
其核心问题是如何计算图像的相似性。
目前视觉SLAM中主要是基于外观的方法，细分下的话，分为传统的方法与基于深度学习的方法，传统的方法有词袋模型与随机蕨法，基于深度学习的方法有CALC算法。 ### **10.2.1词袋模型（Bag of words）** Bag of words模型最初被用在文本分类中，将文档表示成特征矢量。它的基本思想是假定对于一个文本，忽略其词序和语法、句法，仅仅将其看做是一些词汇的集合，而文本中的每个词汇都是独立的。如果文档中猪、马、牛、羊、山谷、土地、拖拉机这样的词汇多些，而银行、大厦、汽车、公园这样的词汇少些，我们就倾向于判断它是一篇描绘乡村的文档，而不是描述城镇的。 > 举例说明 > 文档一：Bob likes to play basketball, Jim likes too. > 文档二：Bob also likes to play football games. > 基于这两个文本文档，构造一个词典：Dictionary = {1:”Bob”, 2. “like”, 3. “to”, 4. “play”, 5. “basketball”, 6. “also”, 7. “football”，8. “games”, 9. “Jim”, 10. “too”}。 > 基于上述的词典可以构造出一个两个直方图向量 > 1：[1, 2, 1, 1, 1, 0, 0, 0, 1, 1] > 2：[1, 1, 1, 1 ,0, 1, 1, 1, 0, 0] > 这两个向量共包含10个元素, 其中第i个元素表示字典中第i个单词在句子中出现的次数. 因此BoW模型可认为是一种统计直方图 (histogram) 在视觉SLAM当中，每个词换为了特征点的描述子，而一幅图像可以理解为由许多描述子形成的文本，然后使用词袋向量来表示这幅图像。在检测是否发生回环时，通过计算每个关键帧与当前关键帧词袋向量的相似度来看是否发生回环。
DBoW系类库是开源的词袋模型库，许多有代表性的VSLAM算法都使用了DBoW做为回环检测算法，如ORB-SLAM系列，VINS-Mono，RTAB-Map。另外，对于激光SLAM，目前也有使用LinK3D描述子的BoW3D词袋库。 ### **10.2.2 随机蕨法（Random ferns）** 开始看到这个名字时，我第一时间想到的是随机森林，但看了算法后，感觉这个和随机森林好像没啥关系。。。
先来看下蕨
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686475423405-10647e9e-2103-40d2-814e-5d0a89bbcec4.png#averageHue=%23568f47&clientId=ud4f25423-17a1-4&from=paste&height=385&id=ub6d1896a&originHeight=702&originWidth=542&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=821142&status=done&style=none&taskId=uff33e8c8-fac2-403a-a5dc-b37c43192ba&title=&width=297.6000061035156)
哦不，先看下随机蕨算法。。。
随机蕨算法是一种图像压缩编码的算法，它首先通过随机算法，对整幅图像随机生成N个采样点，然后对每个采样点，再随机生成一个阈值。然后对每个通道，对比阈值与对应灰度值的大小，生成1或者0的编码，这样每个位置，会生成一个四维的向量。整个图像，会生成一个4N维的向量作为编码，一个随机蕨的生成过程如下：
将F=|fi|，i∈{R,G,B,D}定义为一个随机蕨
$f_{i}=\left\{\begin{array}{l} 1, I_{i}(x) \geqslant \tau_{i} \\ 0, I_{i}(x)<\tau_{i} \end{array}\right.$
式中Ti，的值通过随机函数产生（TR,TG,TB∈[0,255]，TD∈[800,4000])。将随机蕨F中所有的fi按顺序排列，
得到一个二进制编码块bF
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686477100719-63fdd905-225a-46c7-ad35-f482ebc4134a.png#averageHue=%23adc7a9&clientId=ud4f25423-17a1-4&from=paste&height=142&id=ue881f540&originHeight=178&originWidth=410&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=83826&status=done&style=none&taskId=u00c3fe19-11ec-4680-9f84-27ff3db74f5&title=&width=328)
在计算相似度时，使用基于块的汉明距离(block-wise hamming distance: BlockHD)，即只要一个块里有一位不一样，则整个块的距离为1，反之都一样则为0。两幅图像的 BlockHD越小，说明两者越相似，如果 BlockHD值越大，说明差异性越大。使用随机蕨算法来进行相似度计算的有一些RGBD-SLAM算法，如kinect fusion，elastic fusion等 ### **10.2.3 基于深度学习的方法： CALC** CALC是用于回环检测的卷积自动编码器。它该代码分为两个模块。TrainAndTest用于训练和测试模型，DeepLCD是一个用于在线回环检测或图像检索的C ++库。 > 在大型实时SLAM中采用无监督深度神经网络的方法检测回环可以提升检测效果。该方法创建了一个自动编码结构，可以有效的解决边界定位错误。对于一个位置进行拍摄，在不同时间时，由于视角变化、光照、气候、动态目标变化等因素，会导致定位不准。卷积神经网络可以有效地进行基于视觉的分类任务。在场景识别中，将CNN嵌入到系统可以有效的识别出相似图片。但是传统的基于CNN的方法有时会产生低特征提取，查询过慢，需要训练的数据过大等缺点。而CALC是一种轻量级地实时快速深度学习架构，它只需要很少的参数，可以用于SLAM回环检测或任何其他场所识别任务，即使对于资源有限地系统也可以很好地运行。 > 这个模型将高维的原始数据映射到有旋转不变性的低维的描述子空间。在训练之前，图片序列中的每一个图片进行随机投影变换，重新缩放成120×160产生图像对，为了捕捉运动过程中的视角的极端变化。然后随机选择一些图片计算HOG算子，采用固定长度的HOG描述子可以帮助网络更好地学习场景的几何。将训练图片的每一个块的HOG存储到堆栈里，定义为X2，其维度为NxD，其中N是块的大小，D是每一个HOG算子的维度。网络有两个带池化层的卷积层，一个纯卷积层，和三个全连接层，同时用ReLU做卷积层的激活单元。在该体系结构中，将图片进行投影变换，提取HOG描述子的操作仅针对整个训练数据集计算一次，然后将结果写入数据库以用于训练。在训练时，批量大小N设置为1，并且仅使用boxed区域中的层。 > ![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686478526173-d3e45f5c-dfd7-4ec6-a261-c4790894dc73.png#averageHue=%23dededd&clientId=ud4f25423-17a1-4&from=paste&height=242&id=u42d14ec6&originHeight=302&originWidth=1028&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=250754&status=done&style=none&taskId=u7f436cdd-1fae-4fde-9a40-20174138826&title=&width=822.4) ### **10.2.4 基于缩略图** PTAM的回环检测方法和random ferns很像。PTAM是在构建关键帧时将每一帧图像缩小并高斯模糊生成一个缩略图，作为整张图像的描述子。在进行图像检索时，通过这个缩略图来计算当前帧和关键帧的相似度。这种方法的主要缺点是当视角发生变化时，结果会发生较大的偏差，鲁棒性不如基于不变量特征的方法。 目前最广泛使用并且最有效的方法是基于外观的一种方法，使用词袋模型进行回环检测 ## 10.3 词袋模型 词袋是什么，当然是装词的袋子啦。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686573531748-d229ae8f-3b23-4a4e-ba6d-2e5ff32547ba.png#averageHue=%23171f11&clientId=ubabe5fcc-6f45-4&from=paste&height=362&id=udfe237d3&originHeight=453&originWidth=1068&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=300853&status=done&style=none&taskId=u6d612b01-fc00-4d0d-95c5-15bb9393e55&title=&width=854.4)
我们一般只关心袋子里物品的种类和数量，对它们的顺序并不关心。现在我们想问，这袋子容量是多少，能装的物品种类是多少，词袋和回环检测有什么关系？别着急，咱们一个个回答。
**首先是词袋和回环检测的关系：**词袋模型用于回环检测的基本假设是，如果两幅图像中出现的特征种类和数量差不多，那么两幅图像大概率是同一个地方拍摄的。这就是基于外观的回环检测原理。
想象你去商业街购物，在同一家商店里购买东西，出门时袋子里的东西基本是十分相似的，如果去另外一家商店购买，则出门时购物袋里的东西就大概率不一样，当然，也要小心类似于麦当劳和肯德基这种卖同种商品的店铺。因此，我们可以说，词袋相似是回环的必要但不充分条件。
**袋子的容量：**取决于每幅图像提取的特征点数量。而如果袋子能装的东西越多，那包含不同内容的袋子的数量就越多。
**而袋子能装的物品的种类：**取决于我们词库里单词的数量。单词数量越多，每个袋子的差异性就可以越大，不同场景的区分性越好。比如上文的麦当劳和肯德基，如果我们把物品只区分到汉堡这个层面，那我们就无法区分这个袋子是麦当劳的购物袋还是肯德基的购物袋，但如果我们对汉堡进一步分类，香辣鸡腿堡是肯德基的，麦辣鸡腿堡是麦当劳的，炫辣鸡腿堡是汉堡王的，那么我们就可以区分这个购物袋的来源了。
如何表征每个袋子呢，用它里面装的东西就可以了。上图右边两个袋子可以表示为：
**美食街的袋子= 2*青苹果+1*螃蟹+1*汉堡套餐**
**动物园的袋子= 1*大象+1*蝴蝶+2*猫+1*腕龙 **
一看到这两个表示，我们基本可以断定，这肯定不是一个类型的东西，如果看到一个：
**神秘的袋子 = 1*大象+1*蝴蝶+2*猫+1*腕龙 + 1*熊猫**
我们有很大把握说，这个袋子和动物园的袋子来着同一个商店。
那么接下来的问题就是：
1.构建袋子中的这些类别
2.用这些类别物品，表征这个袋子
3.计算两个袋子的相似度
在接下来的几个小节中我们来对每个问题逐一进行讲解。 ### 10.3.1 词的生成 就像生活中常见的做法一样，按照某些特征的不同，我们会把生活中不同物品不断细分为不同类别。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686576641326-11337f61-29fd-4f15-a12f-ed428391a9fd.png#averageHue=%23e2e7f6&clientId=ubabe5fcc-6f45-4&from=paste&height=494&id=ue23ec5ab&originHeight=734&originWidth=617&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=396757&status=done&style=none&taskId=uf00c8853-3541-4770-a0fc-a984a6a7481&title=&width=415.6000061035156)
然后按照这个分类模型，就可以对未知物品，进行分类了。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686576461391-7bcc2686-17a6-446a-8bd9-92a4ea50bfdb.png#averageHue=%23817361&clientId=ubabe5fcc-6f45-4&from=paste&height=268&id=u1cd596bc&originHeight=443&originWidth=617&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=347196&status=done&style=none&taskId=ucd2e69b2-24fe-4735-b0f2-22b98b48349&title=&width=373.6000061035156)
词的构建也是如此，我们先采集大量的图片，然后从中提取出大量描述子，然后对这些描述子，使用聚类的方法，构建出一个一个类出来，每个类，就是一个单词。聚类问题在无监督机器学习（Unsupervised ML）中特别常见，用于让机器自行寻找数据中的规律。BoW的字典生成问题也属于其中之一。首先，假设我们对大量的图像提取了特征点，例如有N个。现在，我们想找一个有k个单词的字典，每个单词可以看作局部相邻特征点的集合，这可以用经典的K-means（K均值）算法解决。 > K-means是一个非常简单有效的方法，因此在无监督学习中广为使用，下面对其原理稍做介绍。简单的说，当有N个数据，想要归成k个类，那么用K-means来做主要包括如下步骤： > 1.随机选取k个中心点：c1，... ，ck。 > 2.对每一个样本，计算它与每个中心点之间的距离，取最小的作为它的归类。 > 3.重新计算每个类的中心点。 > 4.如果每个中心点都变化很小，则算法收敛，退出：否则返回第2步。 但实际训练单词时，我们使用的是和动物学分类中类似的树状形式，进一步说，是K叉树形式。 > 1.在根节点，用K-means把所有样本聚成k类（实际中为保证聚类均匀性会使用K-means++)。这样就得到了第一层。 > 2.对第一层的每个节点，把属于该节点的样本再聚成k类，得到下一层。 > 3.依此类推，最后得到叶子层。叶子层即为所谓的wods。 > 4.计算每个单词的DF权重：idf(i)=logN/n ![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684076464012-3be62e86-088e-4e75-90e2-831b14753b5d.png#averageHue=%23b79b4d&clientId=ucac1b5d4-6cce-4&from=paste&height=335&id=ujQGc&originHeight=419&originWidth=364&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=102881&status=done&style=none&taskId=u1e1356cc-c2db-49da-9107-fe7541e1e10&title=&width=291.2)
实际上，最终我们仍在叶子层构建了单词，而树结构中的中间节点仅供快速查找时使用。这样一个k分支、深度为d的树，可以容纳kd个单词。另外，在查找某个给定特征对应的单词时，只需将它与每个中间节点的聚类中心比较(一共d次)，即可找到最后的单词，保证了对数级别的查找效率。而为每一个单词计算一个IDF权重，主要用来给每个词增加区分度，在数据集中出现得越少的单词，其可辨认度就越高。
我们把生成的这个词汇树，称为字典。训练词汇树的过程就是构建词典的过程。 ### 10.3.2 使用字典表征图像 有了字典之后，给定任意特征，只要在字典树中逐层查找，最后都能找到与之对应的单词wj:当字典足够大时。我们可以认为fi和wj来自同一类物体。那么，假设从一幅图像中提取了N个特征，找到这N个特征对应的单词之后，就相当于拥有了该图像在单词列表中的分布，或者直方图。
另外我们还需要知道每个词的权重。TF-IDF(Term Frequency-Inverse Document Frequency)，或译频率-逆文档频率，是文本检索中常用的一种加权方式，也用于BoW模型中。T部分的思想是，某单词在一幅图像中经常出现，它的区分度就高。另外，IDF的思想是，某单词在字典中出现的频率越低，分类图像时区分度越高。
IDF是在构建字典时就进行的。假设所有特征数量为n，wj的数量为ni，那么该单词的IDF为:
$\mathrm{IDF}_{i}=\log \frac{n}{n_{i}}$
TF部分则是在获得每幅图像时计算的。假设图像A中单词wi出现了ni次，而该幅图像中共出现的单词次数为n，那么TF为
$\mathrm{TF}_{i}=\frac{n_{i}}{n}$
于是，wi的权重等于TF乘IDF之积：
$\eta_{i}=\mathrm{TF}_{i} \times \mathrm{IDF}_{i}$
考虑权重以后，对于某幅图像A，把它的特征点匹配到对应单词之后，组成它的BoW:
$A=\left\{\left(w_{1}, \eta_{1}\right),\left(w_{2}, \eta_{2}\right), \ldots,\left(w_{N}, \eta_{N}\right)\right\} \stackrel{\text { def }}{=} \boldsymbol{v}_{A} \\$
由于相似的特征可能落到同一个类中，因此实际的VA中会存在大量的零。无论如何，通过词袋我们用单个向量）VA描述了一幅图像A。这个向量vA是一个稀疏的向量，它的非零部分指示了图像A中含有哪些单词，而这些部分的值为TF-IDF的值。 ### 10.3.3 相似度计算 接下来的问题是：给定vA和vB,如何计算它们的差异呢？这个问题和范数定义的方式一样，存在若干种解决方式，比如常用的L1范数形式：
$s\left(\boldsymbol{v}_{A}-\boldsymbol{v}_{B}\right)=2 \sum_{i=1}^{N}\left|\boldsymbol{v}_{A i}\right|+\left|\boldsymbol{v}_{B i}\right|-\left|\boldsymbol{v}_{A i}-\boldsymbol{v}_{B i}\right| \\$ ### 10.3.4 回环检测流程 **数据库查询：**
假设通过上节的算法，我们得到了当前图像的词袋（Bow）向量v，现在要在词库中检索最为相似的图像。如何实现这一步，需要先介绍一下DBoW词袋库的特点，以DBoW2词袋库为例：
DBoW2库的结构如下，即保存了图像到词汇（根节点）的正向索引，也保存了词汇到图像的逆向索引。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684076686229-f885b581-e66c-47e2-90fe-16057c77486d.png#averageHue=%23e8cf6f&clientId=ucac1b5d4-6cce-4&from=paste&height=309&id=LNAF0&originHeight=485&originWidth=967&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=170699&status=done&style=none&taskId=u3486b2f4-e398-4025-ab4a-7ca3c7a06fe&title=&width=616)
每当有新图像进来，DBoW2库就会根据描述子的情况，不断更新这个正向索引和逆索引。
在检索最相似的图像时，使用了DBoW2库的逆索引，来加速索引速度，而不是使用暴力匹配的方式：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686582074133-9ac26564-756c-4fb9-b4fc-5d2ee07e3a2b.png#averageHue=%23f0eeec&clientId=ubabe5fcc-6f45-4&from=paste&height=470&id=u07769b3c&originHeight=587&originWidth=577&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=211034&status=done&style=none&taskId=u610a1418-b0f5-45a2-bd96-3aec068107f&title=&width=461.6)
上图的含义，就是对图像Q中每个特征点，通过逆索引找到出现的图像，然后给该图像投一票，到最后哪个图像得票多，哪个就是最相似的图像。
在查询到相似图像后，其得分计算公式如下：
$\eta\left(\mathbf{v}_{t}, \mathbf{v}_{t_{j}}\right)=\frac{s\left(\mathbf{v}_{t}, \mathbf{v}_{t_{j}}\right)}{s\left(\mathbf{v}_{t}, \mathbf{v}_{t-\Delta t}\right)} \\$
其中分母是当前关键帧上一帧，与当前的帧的相似度（最优图像得分），用以对查询结果分数做归一化。但需要注意是在转弯等部分场景，上一帧的相似度与当前帧会很小。为防止分母很小导致得分异常得大，需要对分母做判断，跳过那些得分差的帧，往前找直到得到一个分数合理的关键帧。
**组匹配：**
在进行回环匹配时，会将待匹配的图像按时间划分为独立的组，每一组的相似得分是组内所有图像相似得分之和。这是因为回环帧前后的帧，基本也是相似的，但我们只需要一帧，因此需要进行类似非极大值抑制。
$H\left(\mathrm{v}_{t}, V_{T_{i}}\right)=\sum_{j=n_{i}}^{m_{i}} \eta\left(\mathrm{v}_{t}, \mathrm{v}_{t_{j}}\right)$
1、防止连续图像在数据库查询时存在的竞争关系，但是不会考虑同一地点，不同时间的关键帧。
2、防止误匹配
**时间一致性判断：**
在经过组匹配之后，得到了最佳组匹配，此时需要检验该组匹配的时间一致性。这时做法是，对于当前组匹配vt，与前k帧的最优组匹配，应该有较大的重叠。简单来说，就是在第一次检测到相似帧后，并不会立即认为已发生回环，而是会继续等待接下来k次匹配，在这k次最佳匹配组之间，均有较大的重叠区，这个时候，才会认为真正发生了回环。
在当前经过时间一致性校验之后的匹配组，取组内得分最高的图像，作为匹配图像，进入结构一致性校验。
**结构一致性判断：**
该部分通过特征点匹配，通过RANSAC计算基本矩阵，再通过基本矩阵，对两幅图像的特征点进行投影匹配。这个部分用到了各个特征点在空间中的位置是唯一不变的这一假设。
在匹配时，使用了词袋库维护的正向索引（direct image index），加速了特征点的匹配速度。搜索的时候需要注意，同时为了获取足够数量的特征点，不能直接选取words作为匹配索引。同时凸显特征之间的区分度，也不能采用采用较高层数的节点（词袋树形结构），即需要在合适的某一层进行索引匹配。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686582876028-292b5178-97ac-4972-adee-07d7349c65bb.png#averageHue=%231c1d1c&clientId=ubabe5fcc-6f45-4&from=paste&height=191&id=uc68b4655&originHeight=239&originWidth=878&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=31278&status=done&style=none&taskId=ub7efa937-a1e3-4748-a376-6ffa89e3fdd&title=&width=702.4)
如果使用基础矩阵投影匹配的点数（内点数）大于12个，认为发生了真正的回环。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686662918479-320d4b10-8430-4509-8b06-f31cff4f329e.png#averageHue=%23a8837b&clientId=ud97a95f5-6d2d-4&from=paste&height=246&id=ueaa9c37c&originHeight=307&originWidth=703&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=261493&status=done&style=none&taskId=uac177455-e573-4aba-8b5d-5aefd9d3001&title=&width=562.4) ## 10.4 回环检测算法性能评估 对于回环检测算法的评估，常用的标准就是精度—召回曲线，如下
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686582941537-ae533689-19c5-4a8e-a416-01c917782825.png#averageHue=%23f9f8f7&clientId=ubabe5fcc-6f45-4&from=paste&height=380&id=uc6e47164&originHeight=475&originWidth=905&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=88263&status=done&style=none&taskId=ue2ecacdc-6750-4528-bf7d-cbae5bb8d86&title=&width=724)
评价标准
精度：正确回环检测占总检测回环数目的比率
召回：正确回环检测占测试样本总回环的比率
$\operatorname{Precision}=\frac{\mathrm{TP}}{\mathrm{TP}+\mathrm{FP}} \\ \text { Recall }=\frac{\mathrm{TP}}{\mathrm{TP}+\mathrm{FN}} \\$
一般地，如果某个回环检测算法曲线，被另一个算法的PR曲线包裹在下方，则认为被包裹算法性能弱于包裹其的算法。 ## 10.5 DBoW系列库 **1）DBoW**
DBoW库是一个开源的C++库，用来把图像转换为bag-of-word的表示（词袋模型）。它使用特征提取算法（可选取）提取图像的特征描述子，然后根据一定的策略将这些特征描述子聚类为“单词”，并使用树的形式组织，得到一个vocabulary tree。之后将利用vocabulary tree将图像转换为{单词，权值}的向量表示，通过计算向量间距离的方式计算图像之间的相似性。
源码地址：[https://github.com/dorian3d/DBow](https://github.com/dorian3d/DBow) **2）DBOW2**
DBoW2是DBow库的改进版本，DBoW2实现了具有正序和逆序指向索引图片的的图像数据库，可以实现快速查询和特征比较。与以前的DBow库的主要区别是： - DBoW2类是模板化的，因此它可以与任何类型的描述符一起使用。 - DBoW2可直接使用ORB或BRIEF描述符。 - DBoW2将直接文件添加到图像数据库以进行快速功能比较，由DLoopDetector实现。 - DBoW2不再使用二进制格式。另一方面，它使用OpenCV存储系统来保存词汇表和数据库。这意味着这些文件可以以YAML格式存储为纯文本，更具有兼容性，或以gunzip格式（.gz）压缩以减少磁盘使用。 - 已经重写了一些代码以优化速度。DBoW2的界面已经简化。 - 出于性能原因，DBoW2不支持停止词。 DBoW2需要OpenCV和 Boost::dynamic_bitset类才能使用BRIEF版本。
DBoW2和DLoopDetector已经在几个真实数据集上进行了测试，执行了3毫秒，可以将图像的简要特征转换为词袋向量量，在5毫秒可以在数据库中查找图像匹配超过19000张图片。
源码地址：[https://github.com/dorian3d/DBoW2](https://github.com/dorian3d/DBoW2) **3）DBoW3**
DBoW3是DBow2库的改进版本，与以前的DBow2库的主要区别是： - DBoW3只需要OpenCV。DLIB的DBoW2依赖性已被删除。 - DBoW3能够适用二进制和浮点描述符。无需为任何描述符重新实现任何类。 - DBoW3在linux和windows中编译。 - 已经重写了一些代码以优化速度。DBoW3的界面已经简化。 - 使用二进制文件。二进制文件加载/保存比yml快4-5倍。而且，它们可以被压缩。 - 兼容DBoW2的yml文件 源码地址：[https://github.com/rmsalinas/DBow3](https://github.com/rmsalinas/DBow3) **4）FBOW**
FBOW（Fast Bag of Words）是DBow2 / DBow3库的极端优化版本。该库经过高度优化，可以使用AVX，SSE和MMX指令加速Bag of Words创建。在加载词汇表时，fbow比DBOW2快约80倍。在使用具有AVX指令的机器上将图像转换为词袋时，它的速度提高了约6.4倍。
源码地址：[https://github.com/rmsalinas/fbow](https://github.com/rmsalinas/fbow) ## 本章小结 本章主要介绍了回环检测在SLAM中的意义，即消除里程计的累计误差，获得一张全局一致的地图。并介绍了目前视觉中最为流行的基于外观的回环检测算法，词袋模型。并给出了DBOW3的练习。
下一节我们会介绍VSLAM算法最后一个模块，建图模块，也是与需求端连接最为紧密的一个模块。地图作为一个纽带，连接着建图与定位。 ## 本节思考 验证回环检测算法，需要有人工标记回环的数据集。然而人工标记回环是很不方便的，我们会考虑根据标准轨迹计算回环。即，如果轨迹中有两个帧的位姿非常相近，就认为它们是回环。请根据TUM数据集给出的标准轨迹，计算出一个数据集中的回环。这些回环的图像真的相似吗？ ## 本章练习 [回环检测-词袋模型代码练习](https://github.com/datawhalechina/smoothly-vslam/tree/main/ch10)
[DBoW3仓库](https:/github.com/rmsalinas/DBow3) ## 参考 0.《视觉SLAM十四讲》
[1.回环检测中词袋模型DBoW2的原理分析](https://blog.csdn.net/weixin_37835423/article/details/112890634)
[2.ORB-SLAM3知识点(一)：词袋模型BoW](https://zhuanlan.zhihu.com/p/354616831)
[3.[ORB-SLAM2] 回环&DBoW视觉词袋](https://zhuanlan.zhihu.com/p/61850005)
[4.ORBSLAM2学习（三）：DBoW2理论知识学习](https://blog.csdn.net/lwx309025167/article/details/80524020)
[5.视觉slam中的回环检测概述](https://blog.csdn.net/qq_42518956/article/details/107546479#:~:text=1%20%E6%9C%80%E7%AE%80%E5%8D%95%E7%9A%84%E6%96%B9%E6%B3%95%E6%98%AF%E5%AF%B9%E4%BB%BB%E6%84%8F%E4%B8%A4%E5%B9%85%E5%9B%BE%E5%83%8F%E9%83%BD%E5%81%9A%E4%B8%80%E9%81%8D%E7%89%B9%E5%BE%81%E5%8C%B9%E9%85%8D%EF%BC%8C%E6%A0%B9%E6%8D%AE%E6%AD%A3%E7%A1%AE%E5%8C%B9%E9%85%8D%E7%9A%84%E6%95%B0%E9%87%8F%E7%A1%AE%E5%AE%9A%E5%93%AA%E4%B8%A4%E5%B9%85%E5%9B%BE%E5%83%8F%E5%AD%98%E5%9C%A8%E5%85%B3%E8%81%94%E3%80%82%20%E8%BF%99%E7%A7%8D%E6%96%B9%E6%B3%95%E6%9C%B4%E7%B4%A0%E4%BD%86%E6%98%AF%E6%9C%89%E6%95%88%EF%BC%8C%E7%BC%BA%E7%82%B9%E6%98%AF%E4%BB%BB%E6%84%8F%E4%B8%A4%E5%B9%85%E5%9B%BE%E5%83%8F%E5%81%9A%E7%89%B9%E5%BE%81%E5%8C%B9%E9%85%8D%EF%BC%8C%20%E8%AE%A1%E7%AE%97%E9%87%8F%E5%AE%9E%E5%9C%A8%E5%A4%AA%E5%A4%A7%EF%BC%8C%E5%9B%A0%E6%AD%A4%E4%B8%8D%E5%AE%9E%E7%94%A8%20%E3%80%82,2%20%E9%9A%8F%E6%9C%BA%E6%8A%BD%E5%8F%96%E5%8E%86%E5%8F%B2%E6%95%B0%E6%8D%AE%E8%BF%9B%E8%A1%8C%E5%9B%9E%E7%8E%AF%E6%A3%80%E6%B5%8B%EF%BC%8C%E6%AF%94%E5%A6%82%E5%9C%A8nn%E5%B8%A7%E4%B8%AD%E9%9A%8F%E6%9C%BA%E6%8A%BD5%E5%B8%A7%E4%B8%8E%E5%BD%93%E5%89%8D%E5%B8%A7%E6%AF%94%E8%BE%83%E3%80%82%20%E6%97%B6%E9%97%B4%E6%95%88%E7%8E%87%E6%8F%90%E9%AB%98%E5%BE%88%E5%A4%9A%EF%BC%8C%E4%BD%86%E6%98%AF%E6%8A%BD%E5%88%B0%E5%9B%9E%E7%8E%AF%E5%87%A0%E7%8E%87%E4%B8%8D%E9%AB%98%EF%BC%8C%E4%B9%9F%E5%B0%B1%E6%98%AF%20%E6%A3%80%E6%B5%8B%E6%95%88%E7%8E%87%E4%B8%8D%E9%AB%98%20%E3%80%82)
[6.实时词袋模型BoW3D | 用于3D激光雷达SLAM回环检测（开源）](https://zhuanlan.zhihu.com/p/598151357)
[7.浅谈回环检测中的词袋模型（bag of words）](https://blog.csdn.net/qq_24893115/article/details/52629248)
[8.猫科の完全指南①--猫科动物概论](https://zhuanlan.zhihu.com/p/33842443) ================================================ FILE: docs/chapter11/chapter11.md ================================================ # **11.终识庐山真面目：建图** 学到很多东西的诀窍，就是一下子不要学很多。——洛克 --- > **本章主要内容** \ > 1.地图的类型 \ > 2.稠密重建 \ > 3.点云地图 \ > 4.TSDF地图 \ > 5.实时三维重建 建图的目的是获得满足任务需求的地图。地图的种类十分多，不同类型任务需要不同类型的地图。一般来说，地图可以分为度量地图与拓扑地图，而对于度量地图，又可以继续细分为**栅格地图、特征地图、点云地图等。**
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685861896661-958d6d8f-b02b-4a3b-9efc-d40fa33b68b5.png#averageHue=%23988c7a&clientId=u4d9282ea-6449-4&from=paste&height=374&id=uf8b824d2&originHeight=588&originWidth=1012&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=545664&status=done&style=none&taskId=u940a1d5b-96a6-47a2-b439-e9ffce62079&title=&width=643)
不同类型地图满足不同任务需求：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685805136691-d1cc0485-8ba2-46bd-8cf8-9a946939a9d4.png#averageHue=%23d0cac0&clientId=uc14277d2-25da-4&from=paste&height=384&id=qwQug&originHeight=681&originWidth=1031&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=383126&status=done&style=none&taskId=ub0628d42-67de-4bc7-abd8-0ac199e702f&title=&width=581)
总结来说，地图分类如下：
![](https://cdn.nlark.com/yuque/0/2023/jpeg/1782698/1685863282746-e859afe3-c9d7-4777-b9fd-f15c34e61aa7.jpeg) ## 11.1 度量地图（Metric Map）与拓扑地图（Topological Map） **度量地图**强调精确地表示地图中物体的空间位置，通常用 稀疏（Sparse） 和 稠密（Dense） 对它们进行分类。 - 稀疏地图：进行了一定程度的抽象，并不需要表达所有的物体。例如，选择场景中一部分具有代表意义的东西，称之为路标(Landmark)，那么一张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略。**定位时用稀疏路标地图就足够了**。 - 稠密地图：稠密地图着重于建模所有看到的东西。通常按照某种分辨率，由许多小块组成。二维度量地图是许多小格子（Grid，二维栅格地图），三维中则是许多小方块（Voxel，体素地图）。一个小块一般含有占据、空闲、未知三种状态，以表达该格内是否有物体。当查询某个空间位置时，地图能够给出该位置是否可以通过的信息。这样的地图可以用于各种导航算法，如A*、D*等。 **拓扑地图**则更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性。它放松了地图对精确位置的需要，去掉地图的细节问题，是一种更为紧凑的表达方式。
这里说一下个人对导航和避障的理解，二者是全局路径规划与局部路径规划的关系。全局路径规划需要知道全局静态障碍物的分布，比如墙体，台阶，桌椅这种不会移动的物体的位置，然后在两个点之间基于全局可通行区域搜索出一条路径出来，所以导航注重静态障碍物。避障需要实时感知周围障碍物信息，对突然出现的动态障碍物及时更改路线，因此避障注重动态障碍物，强调实时感知。
对于重建，个人感觉更偏向配准，及如何把点云拼起来，得到一个更还原的三维模型。在这个意义上说，SLAM强调的是通过估计位姿，并通过估计的位姿还原环境，而其地图，更强调拿来做定位。其强调定位准确，而不是得到一个更逼真的三维模型，这是和三维重建的区别所在。所以大多SLAM算法生成的，都是稀疏路标点地图。但通过SLAM估计的位姿及传感器数据，也能获得用于导航的稠密地图。
基于特征点的VSLAM算法建立的地图为稀疏路标点地图，可直接保存描述子信息，使用基于词袋模型的回环检测算法来进行全局定位，这里就不在赘述，接下来着重介绍生成稠密地图。 ## 11.2 稠密建图 由于相机，被认为是只有角度的传感器(Bearing only)。单幅图像中的像素，只能提供物体与相机成像平面的角度及物体采集到的亮度，而无法提供物体的距离(Range)。而在稠密重建中，我们需要知道每一个像素点（或大部分像素点）的距离，对此大致上有如下解决方案： - 1.使用单目相机，估计相机运动，并且三角化计算像素的距离。 - 2.使用双目相机，利用左右目的视差计算像素的距离（多目原理相同）。 - 3.使用RGB-D相机直接获得像素距离。 前两种方式称为立体视觉(Stereo Vision)，其中移动单目相机的又称为移动视角的立体视觉(Moving View Stereo,MVS)。相比于RGB-D直接测量的深度，使用单目和双目的方式对深度获取往往是“费力不讨好”的一计算量巨大，最后得到一些不怎么可靠的深度估计。当然，RGB-D也有一些量程、应用范围和光照的限制，不过相比于单目和双目的结果，**使用RGB-D进行稠密重建往往是更常见的选择**。**而单目、双目的好处是，在目前RGB-D还无法被很好地应用的室外、大场景场合中，仍能通过立体视觉估计深度信息。**
我们从最简单的情况讲起：假设通过SLAM，我们获得了每幅图像对应的相机轨迹，通过这些图像的位姿，估计某幅图像每个像素的深度，这就是最简单的建图问题。
首先，请回忆在特征点部分我们是如何完成该过程的： - 1.对图像提取特征，并根据描述子计算特征之间的匹配。换言之，通过特征，我们对某一个空间点进行了跟踪，知道了它在各个图像之间的位置。 - 2.由于无法仅用一幅图像确定特征点的位置，所以必须通过不同视角下的观测估计它的深度，原理即前面讲过的三角测量。 但在稠密深度图估计中，不同之处在于，**我们无法把每个像素都当作特征点计算描述子**。因此，稠密深度估计问题中，匹配就成为很重要的一环：如何确定第一幅图的某像素出现在其他图里的位置呢？这需要用到极线搜索和块匹配技术2)。当我们知道了某个像素在各个图中的位置，就能像特征点那样，利用三角测量法确定它的深度。不过不同的是，在这里要使用很多次三角测量法让深度估计收敛，而不仅使用一次。我们希望深度估计能够随着测量的增加从一个非常不确定的量，逐渐收敛到一个稳定值。这就是深度滤波器技术。所以，下面的内容将主要围绕这个主题展开。 ### 11.2.1 立体视觉稠密重建 #### 11.2.1.1 立体视觉稠密重建 接上一节，以下两种称为立体视觉(Stereo Vision)，其中移动单目相机的又称为移动视角的立体视觉(Moving View Stereo,MVS)。 - 1.使用单目相机，估计相机运动，并且三角化计算像素的距离。 - 2.使用双目相机，利用左右目的视差计算像素的距离（多目原理相同）。 在单目情况下，通过不同时刻相机位姿进行像素深度估计，而对于双目，则通过左右目图像进行像素深度估计。需要使用极线搜索与块匹配来获取深度分布，然后使用深度滤波器更新深度直到收敛。
第一步，极线计算。由相机之间位姿，计算出该图像上每个像素，在另外一幅图像上的极线。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685867856657-bdc92fd7-9917-4e16-a913-57e2c3240bf5.png#averageHue=%23fdfdfd&clientId=u4d9282ea-6449-4&from=paste&height=295&id=u12e1ee52&originHeight=495&originWidth=666&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=66279&status=done&style=none&taskId=u543a70e5-cf17-4a70-a282-75cde2b8b9c&title=&width=396.79998779296875)
第二步，沿着极线，计算该像素周围纹理，与待匹配点周围纹理的匹配程度。这一步称为块匹配算法。假设我们要计算下图中绿框块与白框块的相似度，可以这么做：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685868680772-d0ec90c1-62fa-4681-b71c-cd93a1ae5c37.png#averageHue=%236b9954&clientId=u4d9282ea-6449-4&from=paste&height=214&id=u3dc8ebdb&originHeight=361&originWidth=889&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=560627&status=done&style=none&taskId=u83740d90-22c3-4a4b-aaf5-eb61b1f2daf&title=&width=527.2000122070312)
先转为灰度图，然后计算每个像素的灰度差的绝对值之和：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685868631759-2b33e9c4-99ec-4323-9b9f-07f3a3aa03c7.png#averageHue=%23f6f6f6&clientId=u4d9282ea-6449-4&from=paste&height=299&id=uf4440814&originHeight=374&originWidth=996&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=98996&status=done&style=none&taskId=uaec095a4-afee-440e-a756-d325f0d41c5&title=&width=796.8)
当然，这里的指标有很多，也可以计算距离平方和（Sum of Squared Distance: SSD）或者归一化相关（Normalized Cross Correlation: NCC)。为了去除光照影响，这一步也可以先减去每个小块的均值，称为去均值的SSD，去均值的NCC等。为了将相机视角对图像的影响考虑进去，也会对匹配的图像块加入仿射变换后，再计算相似度分数。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685867954485-18ab1f5c-d0a0-4ebe-a28b-2cd828044bb7.png#averageHue=%23fcfcfb&clientId=u4d9282ea-6449-4&from=paste&height=319&id=u249d9c06&originHeight=399&originWidth=499&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=74880&status=done&style=none&taskId=u10ec6667-d2d1-47d4-ab7c-da2c043d84b&title=&width=399.2)
第三步，获得最佳匹配点，计算不确定度。获得极线上每个像素点相似度分数可能如下，取最高得分，作为最佳匹配点。这时需要考虑当前匹配点_P__2_下，左右如果误匹配一个像素点，会造成多大的误差，即下图中的P与P'之差，作为方差σobs，而μobs则是这次估计的深度P。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685868024740-be8a0b33-c8cc-4d73-8b1a-ba7007cf6bd9.png#averageHue=%23fdfdfd&clientId=u4d9282ea-6449-4&from=paste&height=330&id=u65bebc7b&originHeight=412&originWidth=590&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=69355&status=done&style=none&taskId=ubbd963a6-a268-469c-a6a8-fbe455eef5e&title=&width=472)
假设深度分布服从高斯分布，则该点深度分布服从：
$P(d_{obs}) = N(\mu _{obs},\delta ^{2}_{obs})$
第四步，更新深度分布。在_P__2_与其他图像的产生了匹配后，按照以上1到3步获得了新的深度分布，则将两个高斯分布进行相乘，得到融合后的深度分布。
$\mu_{f u s e}=\frac{\sigma_{o b s}^{2} \mu+\sigma^{2} \mu_{o b s}}{\sigma^{2}+\sigma_{o b s}^{2}}, \quad \sigma_{f u s e}^{2}=\frac{\sigma^{2} \sigma_{o b s}^{2}}{\sigma^{2}+\sigma_{o b s}^{2}}$
其中σ，而μ是原深度方差及均值。这样不断更新，直到深度不确定度小于阈值，停止更新。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685871375493-05f687ba-40be-4a1f-a937-58a009e5061e.png#averageHue=%23646464&clientId=u4d9282ea-6449-4&from=paste&height=428&id=ubc21395d&originHeight=838&originWidth=1036&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=673146&status=done&style=none&taskId=udb00e5a1-d568-4eea-88c7-0f06349f07a&title=&width=529)
迭代次数越多，深度越稳定。上图是迭代10次后与30次后的效果。 #### 11.2.1.2 稠密重建影响因数分析 - **像素梯度** 对深度图像进行观察，会发现一件明显的事实。块匹配的正确与否依赖于图像块是否具有区分度，即像素梯度变化分布情况。纯色的色块没有灰度梯度，无法得到有效的匹配。产生的深度信息多半是不正确的。如图中打印机表面出现了明显不该有的条纹状深度估计（而根据我们的直观想象，打印机表面肯定是光滑的）。相反，像素梯度比较明显的地方，我们得到的深度信息也相对准确，例如桌面上的杂志、电话等具有明显纹理的物体。这表明了立体视觉中一个非常常见的问题：对物体纹理的依赖性。该问题在双目视觉中也极其常见，体现了立体视觉的重建质量十分依赖于环境纹理。
从某种角度来说，该问题是无法在现有的算法流程上加以改进并解决的。如果我们依然只关心某个像素周围的邻域（小块）的话。进一步讨论像素梯度问题，还会发现像素梯度和极线之间的联系。我们举两种比较极端的情况，一个是像素灰度沿着极线变化（左），一个是像素灰度垂直于极线变化（右）。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685872082581-b113abd2-c621-47ed-bdbe-176e5f30a538.png#averageHue=%23cacaca&clientId=u4d9282ea-6449-4&from=paste&height=364&id=uf0eeeed1&originHeight=757&originWidth=955&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=282433&status=done&style=none&taskId=u3632b015-6d48-4fb5-acc0-02d0efc17f8&title=&width=459)
像素梯度平行于极线方向（像素灰度沿着极线变化，左图），我们能够精确地、确定匹配度最高点出现在何处。而在垂直的情况，即使小块有明显梯度，当我们沿着极线做块匹配时，会发现匹配程度都是一样的，因此得不到有效的匹配。
而实际中，梯度与极线的情况很可能介于二者之间：既不是完全垂直也不是完全平行。这时，我们说，当像素梯度与极线夹角较大时，极线匹配的不确定性大：而当夹角较小时，匹配的不确定性变小。而在演示程序中，我们把这些情况都当成一个像素的误差，实际是不够精细的。考虑到极线与像素梯度的关系，应该使用更精确的不确定性模型。 - **深度分布假设** 从另一个角度看，我们不妨问：把像素深度假设成高斯分布是否合适呢？这里关系到一个参
数化的问题。
逆深度(Inverse depth)是近年来SLAM研究中出现的一种广泛使用的参数化技巧。简单来说，就是深度并不太服从一个高斯分布： - 1.我们实际想表达的是：这个场景深度大概是5~10米，可能有一些更远的点，但近处肯定不会小于相机焦距(或认为深度不会小于0)。这个分布并不是像高斯分布那样，形成一个对称的形状。它的尾部可能稍长，而负数区域则为零。 - 2.在一些室外应用中，可能存在距离非常远，乃至无穷远处的点。我们的初始值中难以涵盖这些点，并且用高斯分布描述它们会有一些数值计算上的困难。 逆深度应运而生。人们在仿真中发现，假设深度的倒数，也就是逆深度为高斯分布。是比较有效的。随后，在实际应用中，逆深度也具有更好的数值稳定性，从而逐渐成为一种通用的技巧。 - **平滑及外点假设** 1.现在各像素完全是独立计算的，可能存在这个像素深度很小，边上一个又很大的情况。我们可以假设深度图中相邻的深度变化不会太大，从而给深度估计加上了空间正则项。这种做法会使得到的深度图更加平滑。
2.我们没有显式地处理外点(Outlier)的情况。事实上，由于遮挡、光照、运动模糊等各种因素的影响，不可能对每个像素都保持成功匹配。而演示程序的做法中，只要NCC大于一定值，就认为出现了成功的匹配，没有考虑到错误匹配的情况。
3.处理错误匹配亦有若干种方式。例如均匀一高斯混合分布下的深度滤波器，显式地将内点与外点进行区别并进行概率建模，能够较好地处理外点数据。
从上面的讨论可以看出，存在许多可能的改进方案。如果我们细致地改进每一步的做法，最后是有希望得到一·个良好的稠密建图的方案的。然而，正如我们所讨论的，有一些问题存在理论上的困难，例如对环境纹理的依赖，又如像素梯度与极线方向的关联（以及平行的情况）。这些问题很难通过调整代码实现来解决。所以，直到目前为止，虽然双目和移动单目相机能够建立稠密的地图，但是我们通常认为它们过于依赖环境纹理和光照，不够可靠。 ### 11.2.2 RGBD稠密重建 除了使用单目和双目相机进行稠密重建，在适用范围内，RGB-D相机是一种更好的选择。对于深度估计问题，在RGB-D相机中可以完全通过传感器中硬件的测量得到，无须消耗大量的计算资源来估计。并且，RGB-D的结构光或飞时原理，保证了深度数据对纹理的无关性。即使面对纯色的物体，只要它能够反射光，我们就能测量到它的深度。这也是RGB-D传感器的一大优势。
利用RGBD进行稠密建图是相对容易的。不过，根据地图形式不同，也存在着若干种不同的主流建图方式。最直观、最简单的方法就是根据估算的相机位姿，将RGB-D数据转化为点云，然后进行拼接，最后得到一个由离散的点组成的点云地图(Point Cloud Map)。在此基础上，如果我们对外观有进一步的要求，希望估计物体的表面，则可以使用三角网格(Mesh)、面片(Surfel)进行建图。另外，如果希望知道地图的障碍物信息并在地图上导航，也可通过体素(Voxl)建立占据网格地图(Occupancy Map)。
使用RGBD相机生成稠密地图时，需要注意如下一些点： - 1.在生成每帧点云时，去掉深度值无效的点。这主要是考虑到RGBD相机会有一个有效量程，超过量程之后的深度值会有较大误差或返回一个零。 - 2.利用统计滤波器方法去除孤立点。该滤波器统计每个点与距离它最近的N个点的距离值的分布，去除距离均值过大的点。这样，就保留了那些“粘在一起”的点，去掉了孤立的噪声点。 - 3.利用体素网络滤波器进行降采样。由于多个视角存在视野重叠，在重叠区域会存在大量的位置十分相近的点。这会占用许多内存空间。体素滤波保证了在某个一定大小的立方体（或称体素）内仅有一个点，相当于对三维空间进行了降采样，从而节省了很多存储空间。 ## 11.3 点云地图 经过以上稠密重建，我们可以获得如下稠密的点云地图。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685874280581-02e42c09-8c95-4b27-9cf2-fe58523b4dd2.png#averageHue=%232e2d2d&clientId=u4d9282ea-6449-4&from=paste&height=295&id=u8eeb5fc3&originHeight=369&originWidth=564&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=239521&status=done&style=none&taskId=ud28be325-abe3-4206-b12e-512e29abc3d&title=&width=451.2)
点云地图提供了比较基本的可视化地图，让我们能够大致了解环境的样子。它以三维方式存储，使我们能够快速地浏览场景的各个角落，乃至在场景中进行漫游。不过，使用点云表达地图仍然是十分初级的，我们不妨按照之前提的对地图的需求，看看点云地图是否能满足这些需求。 - 1.定位需求：取决于前端视觉里程计的处理方式。如果是基于特征点的视觉里程计，由于点云中没有存储特征点信息，则无法用于基于特征点的定位方法。如果前端是点云的ICP,那么可以考虑将局部点云对全局点云进行ICP以估计位姿。然而，这要求全局点云具有较好的精度。我们处理点云的方式并没有对点云本身进行优化，所以是不够的。 - 2.导航与避障的需求：无法直接用于导航和避障。纯粹的点云无法表示“是否有障碍物”的信息，我们也无法在点云中做“任意空间点是否被占据”这样的查询，而这是导航和避障的基本需要。不过，可以在点云基础上进行加工，得到更适合导航与避障的地图形式。 - 3.可视化和交互：具有基本的可视化与交互能力。我们能够看到场景的外观，也能在场景里漫游。从可视化角度来说，由于点云只含有离散的点，没有物体表面信息（例如法线），所以不太符合人们的可视化习惯。例如，从正面和背面看点云地图的物体是一样的，而且还能透过物体看到它背后的东西：这些都不太符合我们日常的经验。 综上所述，我们说点云地图是“基础的”或“初级的”，是指它更接近传感器读取的原始数据。它具有一些基本的功能，但通常用于调试和基本的显示，不便直接用于应用程序。 ### 11.3.1 由点云地图构建网格地图 如果我们希望地图有更高级的功能，那么点云地图是一个不错的出发点。例如，针对导航功能，可以从点云出发，构建占据网格(Occupancy Grid)地图，以供导航算法查询某点是否可以通过。再如，SfM中常用的泊松重建方法，就能通过基本的点云重建物体网格地图，得到物体的表面信息。除了泊松重建，Surfel也是一种表达物体表面的方式，以面元作为地图的基本单位，能够建立漂亮的可视化地图。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685874470287-6d5e1bf6-6f28-488d-ab3a-cec4a9a00f86.png#averageHue=%23bababa&clientId=u4d9282ea-6449-4&from=paste&height=279&id=udfa49d8a&originHeight=349&originWidth=1090&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=240343&status=done&style=none&taskId=ubc3374b9-6f6b-48a0-8d96-5519658ad61&title=&width=872)
点云重建网格：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685874540934-33191df8-3735-4221-ba87-e4b674063148.png#averageHue=%232e2e2e&clientId=u4d9282ea-6449-4&from=paste&height=311&id=u7417c3b3&originHeight=523&originWidth=898&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=480288&status=done&style=none&taskId=u6ee4d869-11f2-444a-98f1-6931aa2bed9&title=&width=533.4000244140625) ### 11.3.2 由点云地图构建八叉树地图 在一个立方体过中心的位置，在上，前，左方向各沿平行于两面的方向切一刀，可以将一个立方体分割为八个小块，然后不断对每个小方块进行分割，每个小块会继续分为八个小块。这相当于每个方块节点展开都会有八个子节点，因此称之为八叉树。每个小立方体是空间的一个元素，称为体素。体素可以表示占据状态，如果该方格内有点云时，该网站占据状态为1，没有占据的话，占据状态为0，还有一种是未知的占据状态。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685881628592-f857a10f-37ff-4965-b92f-e386b1be271e.png#averageHue=%23e9e9e9&clientId=u4d9282ea-6449-4&from=paste&height=222&id=u80e2ec9c&originHeight=455&originWidth=1034&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=245718&status=done&style=none&taskId=u7d75ddf1-2f7e-4abc-b82e-e6507418762&title=&width=505)
这里展开的层数可以设定，层数越多，每个叶子层表示的体素越小，地图“分辨”率越高。看起来会越精细，反之则会越来越粗糙。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685884053753-da379929-3720-4490-aaf6-dc2ab06ad53f.png#averageHue=%23fbfefa&clientId=u4d9282ea-6449-4&from=paste&height=230&id=uf7660aab&originHeight=288&originWidth=683&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=182816&status=done&style=none&taskId=u2d4f7ac0-857b-4604-a714-705d298a8fa&title=&width=546.4)
相比于直接使用点云地图，八叉树地图有如下好处： - 1.减少存储空间。点云文件通常都比较大，相比之下，八叉树存储同样的点云信息则更高效。因为在某一大块立方体内，每个小块都是同一状态时。该大块节点就可以不展开。 - 2.点云地图不能表示直接表示某个空间是否被占据，不能用于导航，而**八叉树地图可以表示某个空间状态，可直接用于导航。** - 3.可以方便地更新空间状态。对于八叉树，我们会选择用概率形式表达某节点是否被占据的事情。例如，用一个浮点数x∈0,1来表达。这个x一开始取0.5。如果不断观测到它被占据，那么让这个值不断增加；反之，如果不断观测到它是空白，那就让它不断减小即可。通过这种方式，我们动态地建模了地图中的障碍物信息。 每一个体素的概率，一般使用对数几率来表示：
$x=\operatorname{logit}^{-1}(y)=\frac{\exp (y)}{\exp (y)+1}$
其中y为当前体素的观测状态，如果该体素不断被观测到为“占据”状态时，则给y加上一个数，反之，则给y减去一个数。当查询概率时，则使用上式，将y转为概率即可。使用公式来说，就是t+1时刻的，对于节点n来说，其概率为开始到t-1时刻的状态，加上t时刻的观测。
$L\left(n \mid z_{1: t+1}\right)=L\left(n \mid z_{1: t-1}\right)+L\left(n \mid z_{t}\right)$
如果要写为概率形式，则比较复杂，这里放在附录中，感兴趣的小伙伴可以自行查看。 ## 11.4 TSDF地图 TSDF是Truncated Signed Distance Function的缩写，与八叉树相似，TSDF地图也是一种网格式（或者说方块式）的地图，先选定要建模的三维空间，按照一定分辨率将这个空间分成许多小块，存储每个小块内部的信息。不同的是，TSDF地图整个存储在显存而不是内存中。利用GPU的并行特性，我们可以并行地对每个体素进行计算和更新，而不像CPU遍历内存区域那样不得不串行。
每个TSDF体素内存储了该小块与距其最近的物体表面的距离。如果小块在该物体表面的前方，则它有一个正值：反之，如果该小块位于表面之后，那么就为负值。TSDF为0的地方就是表面本身或者，由于数值误差的存在，TSDF由负号变成正号的地方就是表面本身。如下图，我们看到一个类似人脸的表面出现在TSDF改变符号的地方。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685884784553-13716e6b-1409-4a02-9c19-48f9e3cb927d.png#averageHue=%23a2a39d&clientId=u4d9282ea-6449-4&from=paste&height=267&id=GnqQR&originHeight=334&originWidth=391&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=123724&status=done&style=none&taskId=u38549b4d-ba92-4bbd-abf5-961e16bd710&title=&width=312.8)
TSDF也有“定位”与“建图”两个问题，与SLAM非常相似，不过具体的形式与本书前面几讲介绍的稍有不同。在这里，定位问题主要指如何把当前的RGB-D图像与GPU中的TSDF地图进行比较，估计相机位姿。而建图问题就是如何根据估计的相机位姿，对TSDF地图进行更新。传统做法中，我们还会对RGB-D图像进行一次双边贝叶斯滤波，以去除深度图中的噪声。
TSDF的定位类似于前面介绍的ICP,不过由于GPU的并行化，我们可以对整张深度图和TSDF地图进行ICP计算，而不必像传统视觉里程计那样必须先计算特征点。同时，由于TSDF没有颜色信息，意味着我们**可以只使用深度图，不使用彩色图就完成位姿估计**，这在一定程度上摆脱了视觉里程计算法对光照和纹理的依赖性，使得RGB-D重建更加稳健。另外，建图部分也是一种并行地对TSDF中的数值进行更新的过程，使得所估计的表面更加平滑可靠。由于我们并不过多介绍GPU相关的内容，所以具体的方法就不展开了，请感兴趣的读者参阅相关文献。 ## 11.5 实时三维重建 实时三维重建是一个与SLAM非常相似但又有稍许不同的研究方向。在前面的地图模型中，以定位为主体。地图的拼接是作为后续加工步骤放在SLAM框架中的。这种框架成为主流的原因是定位算法可以满足实时性的需求，而地图的加工可以在关键帧处进行处理，无须实时响应。定位通常是轻量级的，特别是当使用稀疏特征或稀疏直接法时：相应地，地图的表达与存储则是重量级的。它们的规模和计算需求较大，不利于实时处理。特别是稠密地图，往往只能在关键帧层面进行计算。
但是，现有做法中并没有对稠密地图进行优化。例如，当两幅图像都观察到同一把椅子时，我们只根据两幅图像的位姿把两处的点云进行叠加，生成了地图。由于位姿估计通常是带有误差的，这种直接拼接往往不够准确，比如同一把椅子的点云无法完美地叠加在一起。这时，地图中会出现这把椅子的两个重影一一这种现象被形象地称为“鬼影”。
这种现象显然不是我们想要的，我们希望重建结果是光滑的、完整的，是符合我们对地图的认识的。在这种思想下，出现了一种以“建图”为主体，定位居于次要地位的做法，也就是本节要介绍的实时三维重建。由于三维重建把重建准确地图作为主要目标，所以需要利用GPU进行加速，甚至需要非常高级的GPU或多个GPU进行并行加速，通常需要较重的计算设备。与之相反，**SLAM是朝轻量级、小型化方向发展的**，有些方案甚至放弃了建图和回环检测部分，只保留了视觉里程计。而**实时重建则正在朝大规模、大型动态场景的重建方向发展**。
自从RGB-D传感器出现以来，利用RGB-D图像进行实时重建成为了一个重要的发展方向，陆续产生了Kinect Fusion、Dynamic Fusion、Elastic Fusion、Fusion4D、Volumn Deform等成果。其中，KinectFusion完成了基本的模型重建，但仅限于小型场景；后续的工作则是将它向大型的、运动的，甚至变形场景下拓展。由于篇幅有限，这部分算法在这里就不做展开介绍了，感兴趣的小伙伴可以自行查找了解。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685883833506-009a569d-ecb6-4f5a-ae25-cf7f5c69a9ff.png#averageHue=%235c82b4&clientId=u4d9282ea-6449-4&from=paste&height=270&id=u11c2196b&originHeight=546&originWidth=1056&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=1220820&status=done&style=none&taskId=u3c2cb377-39ae-40ab-ba67-b744288ca7f&title=&width=522)
Kinetic Fusion建图效果
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685883909451-a0033db6-7868-4529-93fc-1f6c3ecba7eb.png#averageHue=%23949494&clientId=u4d9282ea-6449-4&from=paste&height=570&id=udff770ee&originHeight=970&originWidth=786&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=472026&status=done&style=none&taskId=u7f5dc6b9-bf1b-4447-ab48-aa24ae45532&title=&width=461.79998779296875) ## 本章小结 本节介绍SLAM最后一个模块，建图部分。主要介绍了一些常见的地图类型，着重介绍了稠密地图的建图形式与立体视觉如何进行稠密重建。也比较了与SLAM十分类似的一个分支-实时三维重建的特点。 ## 本章思考 1.是否可以只关注梯度显著的区域，进行立体视觉的半稠密重建。
2.如何在八叉树地图中进行导航或路径规划。
3.了解均匀-高斯混合滤波器的原理与实现。 ## 附录 ### 1.八叉树体素网格概率更新公式: $P\left(n \mid z_{1: T}\right)=\left[1+\frac{1-P\left(n \mid z_{T}\right)}{P\left(n \mid z_{T}\right)} \frac{1-P\left(n \mid z_{1: T-1}\right)}{P\left(n \mid z_{1: T-1}\right)} \frac{P(n)}{1-P(n)}\right]^{-1}$ ## 本节代码练习 [smoothly-vslam:单目稠密重建](https://github.com/datawhalechina/smoothly-vslam/tree/main/ch11) ## 参考 1.《视觉SLAM十四讲》
[2.占据栅格地图构建（Occupancy Grid Map）](https://www.guyuehome.com/14968)
[3.基于图像的三维模型重建——稠密点云重建](https://zhuanlan.zhihu.com/p/131590433)
[4.SLAM14讲学习笔记（九）单目稠密重建、深度滤波器详解与补充](https://blog.csdn.net/zkk9527/article/details/89095967#:~:text=%E6%9E%81%E7%BA%BF%E6%90%9C%E7%B4%A2%E5%92%8C%E5%9D%97%E5%8C%B9%E9%85%8D%E7%9A%84%E7%9B%AE%E7%9A%84%E5%B0%B1%E6%98%AF%E6%89%BE%E5%88%B0%E7%89%B9%E5%AE%9A%E7%9A%84%E7%82%B9%E5%9C%A8%E4%B8%8B%E4%B8%80%E5%B8%A7%E4%B8%AD%E7%9A%84%E4%BD%8D%E7%BD%AE%E3%80%82%20%E8%BF%99%E4%B8%AA%E7%9B%AE%E7%9A%84%E5%B0%B1%E6%98%AF%E8%BF%BD%E8%B8%AA%E5%83%8F%E7%B4%A0%E7%82%B9%EF%BC%8C%E5%92%8C%E5%85%89%E6%B5%81%E6%B3%95%E5%92%8C%E7%9B%B4%E6%8E%A5%E6%B3%95%E7%9A%84%E7%9B%AE%E7%9A%84%E6%98%AF%E4%B8%80%E6%A0%B7%E7%9A%84%E3%80%82%20%E8%BF%99%E7%A7%8D%E6%96%B9%E5%BC%8F%E5%B0%B1%E6%98%AF%E5%9F%BA%E4%BA%8E%E6%95%B0%E5%AD%A6%E7%9A%84%E5%87%A0%E4%BD%95%E6%96%B9%E5%BC%8F%EF%BC%9A%20O1p%E8%BF%99%E6%9D%A1%E5%B0%84%E7%BA%BF%E5%9C%A8O2%E5%AF%B9%E5%BA%94%E7%9A%84%E9%82%A3%E4%B8%AA%E6%88%90%E5%83%8F%E5%B9%B3%E9%9D%A2%E4%BC%9A%E5%BD%A2%E6%88%90%E4%B8%80%E4%B8%AA%E6%8A%95%E5%BD%B1%EF%BC%8C%E4%B9%9F%E5%B0%B1%E6%98%AF%E6%89%80%E8%B0%93%E2%80%9C%E6%9E%81%E7%BA%BF%E2%80%9D%EF%BC%8C%E7%BA%A2%E8%89%B2%E7%9A%84%E9%82%A3%E6%9D%A1%E5%AE%9E%E7%BA%BF%E3%80%82%20%E7%89%A9%E4%BD%93%E5%8F%AF%E8%83%BD%E5%9C%A8O1p1%E8%BF%99%E6%9D%A1%E5%B0%84%E7%BA%BF%E4%B8%8A%E7%9A%84%E4%BB%BB%E4%BD%95%E4%BD%8D%E7%BD%AE%EF%BC%8C%E8%BF%99%E4%B8%AA%E4%BD%8D%E7%BD%AE%E6%98%AF%E4%B8%8D%E7%9F%A5%E9%81%93%E7%9A%84%E3%80%82,%E4%BD%86%E6%98%AF%E7%89%A9%E4%BD%93%E6%98%AF%E5%9C%A8%E5%B0%84%E7%BA%BF%E4%B8%8A%E7%9A%84%EF%BC%8C%E5%B0%84%E7%BA%BF%E5%9C%A8%E5%8F%B3%E8%BE%B9%E6%88%90%E5%83%8F%E5%B9%B3%E9%9D%A2%E7%9A%84%E6%8A%95%E5%BD%B1%E5%8F%88%E6%98%AF%E6%9E%81%E7%BA%BF%EF%BC%8C%E9%82%A3%E4%B9%88%E7%89%A9%E4%BD%93%E5%9C%A8%E5%8F%B3%E8%BE%B9%E7%9A%84%E6%88%90%E5%83%8F%E5%B9%B3%E9%9D%A2%E4%B8%8A%E7%9A%84%E6%8A%95%E5%BD%B1%E4%B8%80%E5%AE%9A%E5%9C%A8%E6%9E%81%E7%BA%BF%E4%B8%8A%E3%80%82%20%E4%B8%80%E6%97%A6%E7%A1%AE%E5%AE%9A%E7%89%A9%E4%BD%93%E7%9A%84%E6%8A%95%E5%BD%B1%E5%9C%A8%E6%9E%81%E7%BA%BF%E4%B8%8A%E7%9A%84%E4%BD%8D%E7%BD%AE%EF%BC%8C%E8%BF%9E%E7%BA%BF%E5%B0%B1%E8%83%BD%E7%A1%AE%E5%AE%9A%E7%89%A9%E4%BD%93%E7%9A%84%E7%A9%BA%E9%97%B4%E4%BD%8D%E7%BD%AE%E3%80%82%20%E2%80%9D%E6%9E%81%E7%BA%BF%E6%90%9C%E7%B4%A2%E2%80%9C%E7%9A%84%E5%90%AB%E4%B9%89%E5%B0%B1%E6%98%AF%E5%9C%A8%E6%9E%81%E7%BA%BF%E4%B8%8A%E6%90%9C%E7%B4%A2%EF%BC%8C%E6%89%BE%E5%88%B0%E5%AF%B9%E5%BA%94%E7%9A%84%E6%8A%95%E5%BD%B1%E7%82%B9%E3%80%82%20%E2%80%9D%E5%9D%97%E5%8C%B9%E9%85%8D%E2%80%9C%E7%9A%84%E5%90%AB%E4%B9%89%E5%B0%B1%E6%98%AF%EF%BC%8C%E4%B8%8D%E5%8D%95%E7%8B%AC%E5%8C%B9%E9%85%8D%E7%82%B9%EF%BC%8C%E8%80%8C%E6%98%AF%E5%8C%B9%E9%85%8D%E7%82%B9%E6%89%80%E5%9C%A8%E7%9A%84%E5%B0%8F%E5%9D%97%E7%9A%84%E7%9B%B8%E4%BC%BC%E7%A8%8B%E5%BA%A6%E3%80%82%20%E5%9B%A0%E6%AD%A4%E8%BF%99%E9%87%8C%E5%B0%B1%E5%8F%88%E6%98%AF%E5%9F%BA%E4%BA%8E%E7%81%B0%E5%BA%A6%E4%B8%8D%E5%8F%98%E6%80%A7%EF%BC%8C%E8%B7%9F%E5%85%89%E6%B5%81%E5%92%8C%E7%9B%B4%E6%8E%A5%E6%B3%95%E7%9A%84%E5%81%87%E8%AE%BE%E6%98%AF%E7%9B%B8%E5%90%8C%E7%9A%84%E3%80%82)
[5.三角化---深度滤波器---单目稠密重建（高翔slam---十三讲）](https://www.cnblogs.com/Jessica-jie/p/7730731.html) ================================================ FILE: docs/chapter12/chapter12.md ================================================ # **_12.实践是检验真理的唯一标准：设计VSLAM系统** 人生是没有毕业的学校。——黎凯 --- > **本章主要内容** \ > 1.C++的工程结构 \ > 2.系统设计方法 \ > 3.设计一个VSLAM系统 教程选做环节，本节为教程结束实践环节，按照前面11节内容，设计一个VSLAM系统。
首先是开发语言，由于性能要求，一般SLAM工程需要C++来开发，因此我们的VSLAM系统会以一个C++工程的形式实现。而对于C++工程进行编译，一般使用CMAKE。 - 关于C++工程的目录结构，可以阅读下面这个链接了解： [ cpp-project-structure Public](https://github.com/hattonl/cpp-project-structure) - 关于CMAKE，可以通过这个了解： [CMAKE手册](https://www.zybuluo.com/khan-lau/note/254724)
工欲善其事，必先利其器，一个趁手的开发工具可以加速开发进程。常见的写代码工具有vs code，QT，Clion等。
最后，需要了解运行平台，一般VSLAM算法都是跑在Linux系统上的，即ubuntu系统。在window上，可以通过VMware虚拟机来实现。
基本的准备工作做完了，现在来设计我们的VSLAM系统吧！ ## 12.1 搭建厂房：工程框架确定 大多数Liux库都会按照模块对算法代码文件进行分类存放，譬如头文件会放在头文件目录中，源代码则放在源代码目录中。此外，可能还有配置文件、测试文件、三方库，等等。现在我们按照小型算法库的普遍做法分类，我们的文件：
1.在bin下存储编译好的二进制文件。
2.include/myslam存放SLAM模块的头文件，主要是.h文件。这种做法的目的是，当把包含目录设到include,引用自己的头文件时，需要写include"myslam/xxx.h",这样不容易和别的库混淆。
3.src存放源代码文件，主要是.cpp文件。
4.test存放测试用的文件，也是.cpp文件。
5.config存放配置文件。
6.cmake_modules存放第三方库的cmake文件，在使用g2o之类的库时会用到它。
这样就确定了代码文件的位置。接下来我们要讨论视觉里程计涉及的基础数据结构。 ## 12.2 构建生产流水线：核心算法模块及流程的确定 确定工程框架，相当于把厂房搭建好。而系统的运行，则相当于厂房中的流水线，需要确定有哪些模块，各自负责哪道工序。
那么，我们需要确定这个加工的原材料是什么，有哪些工序，及加工的流程。
既然是VSLAM，那么我们加工的原材料就是图像。我们的上游厂家就是各类相机，比如单目相机，双目相机，RGBD相机等等。单目SLAM难度比较高，对于第一次开厂的我们不太友好，为了控制风险，这次选择加工难度较小的双目相机生产的双目图像。
确定了原材料，接下来就是技术选型了。我们知道一个SLAM系统大致分为 - 核心部分：前端，后端，回环检测，地图模块 - 周边模块：数据接入模块，相机模块，参数配置模块，可视化模块 首先是核心模块设计：
**前端：**
前端主要负责对图像数据提取特征并追踪特征，对当前帧的位姿进行估计，并判断当前帧是否为关键帧。将特征点追踪结果，关键帧的初始估计位姿送到后端进行优化，前端需要实时性较好。
以下是具体技术选型： - 提取特征：为了平衡性能与精度，这里我们选择提取ORB特征。 - 特征追踪：考虑到追踪速度，这里使用光流追踪。 - 位姿估计：基于匀速相机模型，与上一帧建立追踪关系后，我们使用BA来获取当前帧相机的优化位姿。 - 特征点估计：在优化后，追踪结果为内点的特征点，追踪右目图像，并进行三角化获得3D空间坐标。 **后端：**
后端对前端估计的初始位姿及特征点进行局部和全局优化，并返回优化结果，对前端估计的位姿进行修正。后端的计算量很大，主要是修复前端的误差：即位姿估计的误差和错误的数据关联，这部分主流技术使用非线性优化。
以下是具体设计： - 通过条件变量，当前端插入关键帧时，唤醒后端优化线程。 - 使用距离，来剔除最近与较远的关键帧，控制优化的关键帧为7帧。 **回环检测：**
完整的SLAM系统需要回环检测模块以修正长时间运行下的累计漂移，获得全局一致的地图。且回环检测模块可以在建好地图后进行重定位使用。 - 回环检测，使用DBoW3库作为回环检测库。 **地图模块：**
根据我们VSLAM应用的场景，需要确定最后生成何种类型的地图。如果只是定位，则使用路标点地图就可以了，但如果是进一步的导航等其他使用，则需要生成至少是半稠密的地图。
构建的地图主要用以定位，保存描述子信息就可以了。
以下是部分算法框架：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685369544427-386444b2-fc5c-435b-bd05-372e09fc292d.png#averageHue=%23d2d2d2&clientId=ue894490e-9dfa-4&from=paste&height=332&id=u50b9e294&originHeight=415&originWidth=901&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=203744&status=done&style=none&taskId=u13d291b0-bad3-4db6-af1f-e334d5be6ec&title=&width=721) ## 12.3 基本数据结构设计 老的编程思想认为，程序就是算法加数据结构，在上面几节，我们自顶向下设计了工程框架，算法框架，完成了核心算法的设计，接下来最后一步，就是数据结构的设计了。数据结构就需要我们根据各模块的需求，抽象出需要使用的每个类，然后实现这些基本的数据结构。
安装需求，我们需要实现对应于每幅图像的帧这一类，即Frame类，然后是追踪的特征类，即Feature类，保存在地图中的用以定位的路标点，即MapPoint类。还有管理Frame和MapPoint的类，即Map类。 ## 12.4 具体处理逻辑设计 在编码阶段，会涉及具体的处理逻辑。比如前端在根据双目图像确定该帧的位姿时，我们应该怎样使用右目的图像呢？是每一帧都和左右目各比较一遍，还是仅比较左右目之一呢？在三角化的时候，我们是考虑左右目图像的三角化，还是考虑时间上前后帧的三角化呢？实际中任意两张图像都可以做三角化（比如前一帧的左图对下一帧的右图），所以每个人实现起来也会不太一样。
为简单起见，我们先确定**前端的处理逻辑**： - 1.前端本身有初始化、正常追踪、追踪丢失三种状态。 - 2.在初始化状态中，根据左右目之间的光流匹配，寻找可以三角化的地图点，成功时建立初始地图。 - 3.追踪阶段中，前端计算上一帧的特征点到当前帧的光流，根据光流结果计算图像位姿。该算只使用左目图像，不使用右目。 - 4.如果追踪到的点较少，就判定当前帧为关键帧。对于关键帧，做以下几件事： - 提取新的特征点： - 找到这些点在右图的对应点，用三角化建立新的路标点： - 将新的关键帧和路标点加入地图，并触发一次后端优化。 - 5.如果追踪丢失，就重置前端系统，重新初始化。 然后根据这个逻辑，我们就可以做实际的编码了。
**后端运行逻辑**为，在启动之后，将等待map_update的条件变量。当地图更新被触发时，从地图中拿取激活的关键帧和地图点，执行优化。 ## 12.5 效果验证 在编码完成后，我们可以去下载一些公开的双目图像数据集，比如kitti数据集：http://www.cvlibs.net/datasets/.kitti/eval_odometry.php。读取该数据集，通过可视化界面查看系统运行效果。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1686926790766-c37b7eeb-0861-4679-a0a2-aef3cbe1570b.png#averageHue=%23dedddc&clientId=u4752411e-ae7c-4&from=paste&height=271&id=ucebeb0e8&originHeight=613&originWidth=1288&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=186963&status=done&style=none&taskId=u5727b4a4-ec92-4f5a-8358-c8a46702042&title=&width=570)
需要注意的是，练习代码中只包含前端和后端部分，回环检测及地图保存加载模块并未实现，有兴趣的同学可以尝试把这两个模块加到系统中。 ## 本章小结 本章主要介绍一个C++工程的基本结构，和如何自顶向下构建一个实际的VSLAM工程。 ## 本章思考 1.如何把回环模块加入到系统中。
2.如何保存地图及加载地图。 ## 本章练习 [设计VSLAM系统](https://github.com/https://github.com/datawhalechina/smoothly-vslam/tree/main/ch12hu-minghao/smoothly-vslam/tree/main/ch12) ## 参考 0.视觉SLAM十四讲
[1.VS Code 基础教程（一）—— VS Code 的基本使用入门](https://blog.csdn.net/weixin_46215588/article/details/110160065)
[2.CMAKE手册](https://www.zybuluo.com/khan-lau/note/254724) ================================================ FILE: docs/chapter2/chapter2.md ================================================ # **2.差异与统一：坐标系变换与外参标定** 学习这件事不在乎有没有人教你，最重要的是在于你自己有没有觉悟和恒心。——法布尔 --- > **本章主要内容** \ > 1.坐标系变换 \ > 2.相机外参标定 上一章我们了解了相机内参的概念，内参主要解决三维世界与二维图像之间的映射关系。有了内参我们可以一定程度上还原相机看到了什么（但缺乏尺度）。但相机看到的数据只是处于相机坐标系，为局部观测，我们需要将局部观测转换到全局观测上，这就涉及坐标系间转换。将传感器坐标系观测转换到载体坐标系需要通过外参。本章将介绍坐标系转换及相机外参这两部分内容。 ## **2.1 坐标系转换** ![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986314191-4c2666d6-d0cc-48d5-a49c-f247f1eb7760.png#averageHue=%23f5f4f4&from=url&height=356&id=rKEIc&originHeight=451&originWidth=847&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=668)
如图6，对同一物体，在不同坐标系下便有不同坐标。那么如果知道A与B的相对位置关系，且知道树在A坐标系下的坐标，我们如何求出树在B坐标系下的坐标呢，这就涉及到坐标系转换的内容。
首先我们有一个人为设定的世界坐标系，任何物体在这个坐标系下有其固定的坐标，这个是不变的。而这个物体被相机观测到之后，在相机坐标系下会有一个观测坐标，如图6中的A,B,C,但这个观测是相对观测，只对当前的相机坐标系有意义，对其他观测者是没有意义的，因为它无法把这个信息转换为对自己有用的信息，只有当坐标能转化到世界坐标系这个公共坐标系下，其他观测者才能利用这次观测，从而达到对环境的“有效建模”。
那么如何转换呢，需要使用转换矩阵来完成。接下来我们会从最简单的二维坐标系变换开始，一步一步推导出三维坐标系之间坐标转换的计算方式。 ### **2.1.1 二维纯旋转变换** ![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986314591-1331f5eb-a9c0-44fb-a8e5-0ee850a394ad.png#averageHue=%23fcfafa&from=url&height=307&id=J4U8V&originHeight=425&originWidth=535&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=386.98931884765625)
假设XOY为固定的世界坐标系，X'OY'为活动坐标系，在XOY坐标系下有一点P(x,y),假设X'OY'相对于XOY旋转了θ度，则根据三角函数，可以得到在X'OY'下点P的坐标P'为：
$\mathbf{x}^{*}=\mathbf{O D}+\mathrm{DF}=\mathbf{x} \times \cos (\theta)+\mathbf{y} \times \sin (\theta) \\ \mathbf{y}^{*}=\mathbf{P C}-\mathrm{FC}=y \times \cos (\theta)-\mathbf{x} \times \sin (\theta) \\$
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986314948-23724978-7359-4f82-bb71-2ba13cfa17b4.png#averageHue=%23fcfcfb&from=url&height=397&id=vngjH&originHeight=483&originWidth=473&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=388.9967956542969)
根据上述计算关系，我们也可以把P看做首先在X'OY'中测得的坐标，而XOY相对于X'OY'反向旋转了θ度（-θ），可以得到在活动坐标系下点P(x,y)，在固定世界坐标系下坐标为
$\mathbf{x}^{*}=\mathbf{O D}+\mathrm{DF}=\mathbf{x} \times \cos (-\theta)+\mathbf{y} \times \sin (-\theta)=\mathbf{x} \times \cos (\theta)-\mathbf{y} \times \sin (\theta) \\ \mathbf{y}^{*}=\mathbf{P C}-\mathbf{F C}=y \times \cos (-\theta)-\mathbf{x} \times \sin (-\theta)=y \times \cos (\theta)+\mathbf{x} \times \sin (\theta) \\$
写为矩阵形式为
$\left(\begin{array}{l} x^{*} \\ y^{*} \end{array}\right)=\left(\begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{l} x \\ y \end{array}\right)=R\left(\begin{array}{l} x \\ y \end{array}\right) \\$
其中R为X'OY'在XOY坐标系下的旋转矩阵 ### **2.1.2二维纯平移变换** 两个坐标系只有平移变换，则坐标系转换很简单，如下
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986315252-1f31c1b4-49b3-4b0d-83c3-93c3034164e3.png#averageHue=%23fafafa&from=url&height=329&id=pLtT9&originHeight=466&originWidth=571&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=402.9754333496094)
$\mathbf{x}^{*}=\mathbf{x}+t_{x} \\ \mathbf{y}^{*}=\mathbf{y}+t_{y} \\$ ### **2.1.3二维坐标系变换** ![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986315567-78af370f-58a6-43c0-ad78-a0abc6bc07a9.png#averageHue=%23fdfcfc&from=url&height=385&id=CoZzY&originHeight=597&originWidth=717&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=462.9668884277344)
把上述旋转变换与平移变换结合在一起，就是二维坐标系变换
$\left(\begin{array}{l} x^{*} \\ y^{*} \end{array}\right)=\left(\begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{l} x \\ y \end{array}\right)+\left(\begin{array}{l} t_{x} \\ t_{y} \end{array}\right) \\$ ### **2.1.4 最终坐标系变换-扩展到三维** 二维的旋转，可以看做三维下只绕Z轴的旋转，其中Z坐标不变，旋转矩阵扩展为
$\left(\begin{array}{l} x^{*} \\ y^{*} \\ z^{*} \end{array}\right)=\left(\begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \\ z \end{array}\right) \\$
相应的，对于不同旋转，目前只需要记住中间有一个3X3的旋转矩阵就行（该矩阵有一个特殊性质，就是属于SO3群），而平移则是从二维变为三维就行了，最后三维变换：
$\left(\begin{array}{l} x^{*} \\ y^{*} \\ z^{*} \end{array}\right)=\left(\begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \\ z \end{array}\right)+\left(\begin{array}{l} t_{x} \\ t_{y} \\ t_{z} \end{array}\right)=R\left(\begin{array}{l} x \\ y \\ z \end{array}\right)+t \\$
但对于坐标系变换，常用齐次矩阵来表示，及把旋转变换与平移变换，放到一个矩阵里
$\left(\begin{array}{l} x^{*} \\ y^{*} \\ z^{*} \\ 1 \end{array}\right)=\left(\begin{array}{cccc} \cos \theta & -\sin \theta & 0 & t_{x} \\ \sin \theta & \cos \theta & 0 & t_{y} \\ 0 & 0 & 1 & t_{z} \\ 0 & 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \\ z \\ 1 \end{array}\right) \\$
在原三维向量下，添加1后的坐标，称为齐次坐标，这个是几何变换中常用的数学技巧
$\widetilde{a^{*}}=\left(\begin{array}{cc} R & t \\ 0 & 1 \end{array}\right) \widetilde{a} \\$
于是可以简化到下面这种形式：
$\widetilde{a_{1}}=T_{2}^{1} \widetilde{a_{2}} \\$
其中T12称为变换矩阵，表示将坐标系2下的坐标，转换到1所需要变换。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986315890-867123ff-6466-4ee4-ad65-af6c568907a3.png#averageHue=%23fefdfd&from=url&height=330&id=JUMVx&originHeight=561&originWidth=1038&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=611.2622680664062)
使用坐标系变换，可以把相机坐标系（cam）下的坐标，转换到世界坐标系下。反之，也可以把世界坐标系下坐标，转换到相机坐标系下。 ## **2.2 相机外参** ### **2.2.1 定义** 对于移动载体来说，相机只是载体的一个传感器，我们更感兴趣的是，通过相机观测到的物体，相对于载体而言，其坐标为什么。如果说相机坐标系为camera_link，那么载体坐标系为base_link，这二者之间存在一个变换矩阵，通常用camera_link到base_link的欧式转换表示，而这个变换矩阵，就是常说的相机外参。使用外参矩阵，可以把相机坐标系下看到的点转换到机器人坐标下。
通俗来讲，相机外参描述了相机安装在载体的什么地方，安装角度是怎样的。
如下图，相机0与相机1均不与飞行器本体坐标系重叠，假设相机0安装在本体坐标系的t处（平移），旋转矩阵为R，则其外参为（R,t）。通过相机外参，可以把相机观测转换到飞行器本体坐标系的观测中。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986316336-769c72ba-42e2-4ca6-b5ab-809cc85609e4.png#averageHue=%23b3b3b3&from=url&height=365&id=ljrCP&originHeight=726&originWidth=1132&originalType=binary&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&title=&width=569.2633666992188) ### **2.2.2 相机外参的标定** 如果知道相机相对于载体坐标系安装的位置与角度，是可以直接得到相机外参的，但是，由于机械安装的误差，或者运动过程的变形，这个外参是会改变的。为了获取此时的外参，常借助于一些外参标定工具，常见的外参标定工具有如下几种：
**OpenCV**
OpenCV除了提供相机内参标定外，也提供了相机外参标定的功能。通过拍摄已知的三维空间点的图像，并利用这些点在相机坐标系下的三维坐标，可以计算相机的位姿。
**Matlab**
Matlab也提供了相机外参标定的工具箱，其中包括了单目和立体相机的标定工具。通过拍摄已知的三维空间点的图像，可以计算相机的位姿。
**Kalibr**
Kalibr是ETH Zurich的一个开源相机标定工具，支持单目、双目和多目相机标定。Kalibr使用基于优化的方法来计算相机的位姿，同时也可以进行相机和IMU的联合标定。 ### **2.2.3 外参标定的意义** 相机外参标定确定了相机在世界坐标系中的位置和朝向，这些信息可以用于将图像中的点转换为世界坐标系中的点，或者将世界坐标系中的点转换为图像中的点。因此，外参标定对于相机视觉算法的准确性和稳定性有着重要的影响。以下是外参标定对算法的几个方面的影响： 1. 特征匹配：在进行特征匹配时，需要将图像中的特征点匹配到世界坐标系中的点。如果外参标定不准确，将导致匹配错误，从而影响算法的准确性。 2. 三维重建：在进行三维重建时，需要将多张图像中的点匹配到世界坐标系中的点，并进行三维重建。如果外参标定不准确，将导致重建的几何形状不准确或者出现扭曲。 3. 目标跟踪：在进行目标跟踪时，需要将跟踪目标在多张图像中的位置匹配到世界坐标系中的点，并进行跟踪。如果外参标定不准确，将导致跟踪不准确或者跟踪失败。 4. 相机姿态估计：在进行相机姿态估计时，需要将相机在多张图像中的位置和朝向匹配到世界坐标系中的点，并进行姿态估计。 ### **2.2.4 相机标定类型** 相机标定是指确定相机内部参数和外部参数的过程，以便在图像中恢复真实世界中的几何信息。常用的相机标定类型包括以下几种： 1. **内部标定：**用于确定相机的内部参数，包括焦距、主点位置、畸变系数等。内部标定通常使用标定板等已知几何形状的物体，并采用校正方法进行计算。 2. **外部标定：**用于确定相机在世界坐标系中的位置和方向。外部标定通常使用已知位置的标定板或者其他几何形状的物体，并采用三维重建方法进行计算。 3. **传感器到传感器标定：**用于确定多个相机之间的相对位置和方向，以便进行双目或多目视觉处理。传感器到传感器标定通常使用标定板或者球形标定物等，采用立体匹配算法进行计算。 4. **传感器到车体标定：**用于确定相机在车体坐标系中的位置和方向，以便进行车载视觉处理。传感器到车体标定通常使用车体固定的标定板或者其他几何形状的物体，采用三维重建方法进行计算。 5. **姿态标定：**用于确定相机在平面或者空间中的朝向，以便进行视觉导航或者机器人控制。姿态标定通常使用旋转平台或者陀螺仪等设备，采用角度解算方法进行计算。 6. **相机-激光雷达标定：**用于确定相机和激光雷达之间的相对位置和方向，以便进行三维点云重建或者障碍物检测。相机-激光雷达标定通常使用已知几何形状的标定板或者球形标定物，采用多视角几何约束方法进行计算。 7. **相机-IMU标定：**用于确定相机和惯性测量单元（IMU）之间的相对位置和方向，以便进行惯性辅助导航或者姿态估计。相机-IMU标定通常使用运动平台或者旋转平台等设备，采用卡尔曼滤波或者优化方法进行计算。 ## 本章小结 本章主要介绍如何将相机坐标系下观测，转换到其他坐标系（如载体坐标系），紧接着介绍了相机外参。相机外参主要作用是将相机的局部观测转到载体坐标系下。载体在世界坐标系中不断运动，如何描述载体在世界坐标系下姿态，见下一章。 ## 本章思考 1.相机外参的作用是什么？
2.一个载体搭载着摄像头，在平面上运动，相机外参【t(x,y,z)与R(roll,pitch,yaw)】哪些量不可以被标定出来？ ## **附录** ### **1. 摄像机外参标定** 求解相机的内、外参数矩阵，描述了三维世界到二维像素的映射关系
![image-20230618182716202.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687086475049-cd4eef91-abaa-464a-9ae8-6de4dba6dc6d.png#averageHue=%23f7eeee&clientId=u71ec186e-6a17-4&from=ui&height=206&id=x9mdn&originHeight=343&originWidth=828&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=26603&status=done&style=none&taskId=uae023d40-1a86-4d11-98e8-fdb5739cdb4&title=&width=498.2553405761719)
其中的$\theta$为单个像素两条边的夹角，一般我们认为这个角度为90°，早期对于一些加工不好的传感器，像素可能会是平行四边形。
M矩阵有11个未知量，求解投影矩阵需要最少六对点对应，但实操时一般使用多于六对点获得更鲁棒的效果
**径向畸变**：图像像素点以畸变中心为中心点,沿着径向产生的位置偏差,从而导致图像中所成的像发生形变
**产生原因**：光线在远离透镜中心的地方比靠近中心的地方更加弯曲
图像放大率随距光轴距离的增加而减小
![image-20230618184958384.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687086523239-1b925afa-793c-48a6-b2ff-ff6852f18f0c.png#averageHue=%23e4e2e2&clientId=u71ec186e-6a17-4&from=ui&height=211&id=bgmvz&originHeight=272&originWidth=314&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=41856&status=done&style=none&taskId=u974980f4-b7a4-4da9-8b7d-6e305a86e49&title=&width=243.9957275390625)
![image-20230618190527055.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687086536321-2688ffe1-89d9-4f41-b6e0-8cf45409f642.png#averageHue=%23f4efef&clientId=u71ec186e-6a17-4&from=ui&height=178&id=gS21G&originHeight=398&originWidth=665&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=14764&status=done&style=none&taskId=u23300135-aec4-49f9-93b0-ea855d099d4&title=&width=296.9786376953125)
![image-20230618190543351.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687086549357-93f8a9be-dc3f-46c1-86b9-e950f81a4e19.png#averageHue=%23f2eded&clientId=u71ec186e-6a17-4&from=ui&height=132&id=BXqcf&originHeight=241&originWidth=748&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=23543&status=done&style=none&taskId=u877ebc0a-b417-4573-b5de-2becf9b7f35&title=&width=409.2339782714844) ### **2 提取摄像机参数** 将投影矩阵M拆分开，参数分为内参数与外参数。
![image-20230618182716202.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687086475049-cd4eef91-abaa-464a-9ae8-6de4dba6dc6d.png#averageHue=%23f7eeee&clientId=u71ec186e-6a17-4&from=ui&height=147&id=IC3ye&originHeight=343&originWidth=828&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=26603&status=done&style=none&taskId=uae023d40-1a86-4d11-98e8-fdb5739cdb4&title=&width=355)
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687676964325-f3bc3aec-baa5-49d9-bbf5-7dc8e737de5d.png#averageHue=%23f2f1f1&clientId=u04f2e5be-db77-4&from=paste&height=99&id=ue5d7241a&originHeight=225&originWidth=1306&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=35034&status=done&style=none&taskId=u8d0874bc-c0ca-46a3-b300-31fe85ea799&title=&width=573.2625122070312)
对于这里的$\rho$，是一个标量，因为我们求得的解，其实会与实际的解相差一个系数，该系数由$\rho$表示。 #### **2.1 提取摄像机内参数u、v与**θ ![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687677759430-8a1f8808-a8dd-421b-a2c5-1037c7a8abdd.png#averageHue=%23eee3e3&clientId=u04f2e5be-db77-4&from=paste&height=76&id=u8517ccee&originHeight=145&originWidth=721&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=12803&status=done&style=none&taskId=u06843a5d-2646-4eda-9ce1-095b862deea&title=&width=379.2104797363281)
其中A与b分别为
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687677796721-87527164-50f8-4f44-af83-aa9fbf0a3b1d.png#averageHue=%23ebebeb&clientId=u04f2e5be-db77-4&from=paste&height=100&id=u5c476f54&originHeight=117&originWidth=280&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=5667&status=done&style=none&taskId=u94e8e4a7-e06b-4bef-8793-025d24038a0&title=&width=239.31624809430699)
带入其中得
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687677822442-9bfb0f99-555a-4399-b302-9f312f2c09c9.png#averageHue=%23f4c291&clientId=u04f2e5be-db77-4&from=paste&height=128&id=uc8b03685&originHeight=198&originWidth=809&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=22926&status=done&style=none&taskId=u9259c8ed-baf5-49d5-972e-78e5e513518&title=&width=522.9930419921875)
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687677871259-0ca39e11-01ce-42eb-8011-a234b2b9a3b4.png#averageHue=%23f1f1f1&clientId=u04f2e5be-db77-4&from=paste&height=113&id=ud3109a73&originHeight=169&originWidth=925&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=20719&status=done&style=none&taskId=u57d9e961-27fa-42f3-a1f8-b94d18c8ce9&title=&width=620.2708129882812)
其中K、R、T分别为
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687677909559-ffdb9fc3-d862-4cac-825e-e05faa14af27.png#averageHue=%23f1f1f1&clientId=u04f2e5be-db77-4&from=paste&height=215&id=ud06cc5c8&originHeight=251&originWidth=284&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=10681&status=done&style=none&taskId=u6ad92aed-d703-4beb-8640-d7175bb0f5f&title=&width=242.73505163851138)
计算得
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687677951401-0426a555-ce42-4e4b-b7b0-bbdc64cebb39.png#averageHue=%23ececec&clientId=u04f2e5be-db77-4&from=paste&height=81&id=u0922c0f9&originHeight=142&originWidth=560&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=11577&status=done&style=none&taskId=u75c6838d-a01f-43f8-934d-7c6753919e6&title=&width=319.5908203125)
利用正交性质，将两行对应相乘
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687678321270-2765e855-55ad-45e6-9050-f31d6ca647eb.png#averageHue=%23ededed&clientId=u04f2e5be-db77-4&from=paste&height=101&id=u85a1810a&originHeight=167&originWidth=561&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=14415&status=done&style=none&taskId=u497ef56f-2a49-4d8a-9c6f-0510d7b0647&title=&width=339.4620666503906)
求相应的模，可得
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687678463041-b9486499-1d71-450e-892b-3669e1621a5c.png#averageHue=%23efeaea&clientId=u04f2e5be-db77-4&from=paste&height=116&id=ud12ea3f9&originHeight=204&originWidth=366&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=10748&status=done&style=none&taskId=u07f52633-5635-4aae-87f5-52ffc6158b2&title=&width=208.8076934814453)
自己点乘自己，可得
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687678560299-b651ba5a-d8b5-459f-9cfc-f4a1e40ce646.png#averageHue=%23ffeded&clientId=u04f2e5be-db77-4&from=paste&height=105&id=u5cf59678&originHeight=151&originWidth=788&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=17364&status=done&style=none&taskId=u9126a13c-42d2-4e06-9b11-4d8e1c1568f&title=&width=547.4989624023438)
得到θ
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687678626948-278dfc28-1258-4c9d-aeeb-7ad35e166c3b.png#averageHue=%23e5e5e5&clientId=u04f2e5be-db77-4&from=paste&height=66&id=u364d71a0&originHeight=108&originWidth=556&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=8666&status=done&style=none&taskId=ua1c8b081-84b4-4551-928a-a8a03b2a11c&title=&width=341.1949768066406)
最后得到α与β
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687678784786-47a8e0d7-02b1-4f7d-8316-943ad6f38895.png#averageHue=%23e4e4e4&clientId=u04f2e5be-db77-4&from=paste&height=75&id=u07793813&originHeight=138&originWidth=444&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=10637&status=done&style=none&taskId=u9da8446f-1cbe-4722-bcf6-bcab101769c&title=&width=240.45086669921875) #### **2.2 提取摄像机外参数r1、r2与**r3 ![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687679100354-2365f32c-9303-44c5-8090-2f081f967ff4.png#averageHue=%23eeeeee&clientId=u04f2e5be-db77-4&from=paste&height=105&id=ue5c0ed2e&originHeight=197&originWidth=559&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=12550&status=done&style=none&taskId=u4edf7467-b424-4b05-bd9e-9aaebc8cc39&title=&width=296.72650146484375)
再求得外参数T
![image.png](https://cdn.nlark.com/yuque/0/2023/png/36012760/1687679171442-d89aff6e-6004-46c3-b082-2f749fa320ba.png#averageHue=%23e5e5e5&clientId=u04f2e5be-db77-4&from=paste&height=30&id=uc3479adf&originHeight=59&originWidth=256&originalType=binary&ratio=1.1699999570846558&rotation=0&showTitle=false&size=2864&status=done&style=none&taskId=uc22e8968-178a-46ee-84a2-ad3e0dffeee&title=&width=131.7911376953125) ## **参考** [1.坐标变换最通俗易懂的解释（推到+图解）](https://blog.csdn.net/weixin_45590473/article/details/122848202)
[2.计算机视觉之三维重建（深入浅出SfM与SLAM核心算法）——2.摄像机标定_哔哩哔哩_bilibili](https://www.bilibili.com/video/BV1DP41157dB?p=2&vd_source=6deef2eecab362ce87a170f121391888) ================================================ FILE: docs/chapter3/chapter3.md ================================================ # **3.描述状态不简单：三维空间刚体运动** 知识就是力量。——培根 --- > **本章主要内容** \ > 1.世界坐标系与里程计坐标系 \ > 2.三维刚体运动 \ > 3.旋转的参数化 三维空间刚体的运动，简单来说由平移与旋转构成。平移指两个点之间的位移，三维空间中其自由度为三。旋转则是绕着三个轴各种旋转的角度，自由度也为三。平移加旋转可构成一次变换，这个变换称为欧式变换。
在上节中我们介绍了坐标系变换。其实三维空间的刚体运动与其有紧密的联系，可以理解为刚体运动为持续地把运动坐标系中的坐标变换到固定坐标系之中。但区别是需要加上旋转，那接下来我们就看一下这个具体过程。 ## 3.1 世界坐标系与里程计坐标系 ![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1688892099466-75bc1fdb-bcb6-4f5b-b7c2-1ed2d5367a8b.png#averageHue=%23f3f2f2&clientId=u0f99556f-3fe0-4&from=paste&height=191&id=u06d02566&originHeight=299&originWidth=918&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=16700&status=done&style=none&taskId=u74894042-b1a5-4c38-9cf6-2cb9f068c1e&title=&width=587.4000244140625)
世界坐标系：世界坐标系（world坐标系）一般指固定坐标系，在系统运行过程其全局坐标不发生改变。在建图时，经常会认为起点为世界坐标系原点，而本体初始旋转角度为0。
里程计坐标系：里程计坐标系一般指运动坐标系，在建图过程一般指传感器坐标系，视觉SLAM中指的就是相机坐标系（camera坐标系）。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1688892946337-2b46e7c4-e9b1-4ae9-8876-43e0a07e69ff.png#averageHue=%23f5f4f4&clientId=u0f99556f-3fe0-4&from=paste&height=263&id=ua88239c7&originHeight=329&originWidth=1079&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=24827&status=done&style=none&taskId=u19b433db-5e7e-4008-847c-f4f2e9890bd&title=&width=863.2)
与坐标系变换的区别与联系。坐标系变换中，向量A不动，在已知A在C1坐标系下的坐标，如何转换到在C2坐标系视角下，求其在C2坐标系下坐标，或者转到w坐标系，即世界坐标系下，求A在w视角下的坐标。但这个过程没有对向量A的姿态是没有进行转换的。
在描述刚体运动时，我们一般会求得下一个时刻，相对于上一个时刻坐标系下的平移与旋转，如上图右图所示。这相当于我们知道了c2在c1坐标系下的坐标（平移）与相对于c1的旋转（局部坐标）。这个时候，利用c1在w下的坐标，将c2的平移与旋转转换到世界坐标系下，就得到了c2时刻的全局坐标。然后对于c2，得到下一时刻c3相对于c2的平移与旋转（局部坐标），将其转换到w坐标系下，不断进行下去，就得到了以运动坐标系表征的本地在世界坐标系下的刚体运动。 ## 3.2 欧式变换的累加 假设c1原点在w坐标系下坐标为P1，旋转为R1，c2原点在c1坐标系下坐标为△P，旋转为△R，则c2在w坐标系下的欧式变换P2与R2为：
P2 = R1*△P+P1
这一步与坐标系变换是一样的，接下来是旋转。
R2 = R1*△R;
因为△R是相对于c1坐标系下的旋转，所以是右乘。关于旋转左乘与右乘的区别的，见参考目录。
欧式变换为一个平移+一个旋转，为了简洁，可以使用一个矩阵表示：
$T = \left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\ \boldsymbol{0}^{\mathrm{T}} & 1 \end{array}\right]$
那么上述变换，就可以写为：
$T_2 = \left[\begin{array}{ll} \boldsymbol{R}_1 & \boldsymbol{t}_1 \\ \boldsymbol{0}^{\mathrm{T}} & 1 \end{array}\right]\left[\begin{array}{ll} \bigtriangleup \boldsymbol{R} & \bigtriangleup \boldsymbol{t} \\ \boldsymbol{0}^{\mathrm{T}} & 1 \end{array}\right] =T_1 \bigtriangleup T^{1}_{2}$
可以看到，只要求得当前时刻，在上一坐标系下的位移及旋转，就可以得到当前时刻的全局欧式变换。不断累加下去，就可以得到所有时刻的欧式变换，连起来就是传感器在三维空间的刚体运动。在加上上一节的外参变换，就可以得到本体在空间中的刚体运动了。 ## 3.3 平移与旋转的表示 ### 3.3.1平移 平移的表示比较简单，就是一个三维向量t(x,y,z)，需要注意的是，平移为一个向量。 ### 3.3.2旋转 > 特殊正交群SO(3)，也称旋转群，表示在复合作用下绕原点旋转的群。旋转是保持向量长度和相对向量方向（即旋向性）的线性变换。它在机器人学中的重要性在于表达刚体在3D空间中的旋转：一个rigid motion（刚体运动） 精确地要求刚体在运动时必须保持距离，角度和相对方向。否则，如果不能保持模长，角度或相对方向，则不能认为物体是刚性的。 旋转群通常由一组旋转矩阵表示。但是，四元数也组成了旋转群的一个很好表达。这里首先介绍SO(3)中的旋转矩阵和四元数。但这两种旋转表达缺乏几何直观，对人类交互不太优化。大数学家欧拉发明了欧拉角和轴角来表达旋转，这两者可以与SO(3)群相互转换。另外，对于SO(3)群，其没有加法，在优化中不方便对旋转变量进行调整，需要使用到其对应的李代数进行转化，这部分内容本节不做讲解，感兴趣的同学可以看参考目录4([4.第四讲：李群和李代数](https://zhuanlan.zhihu.com/p/33156814))。
补充阅读：
三维空间里表述旋转的计算方式常见的有两种：矩阵(Matrix)和四元数（Quaternion），为了防止矩阵方式存在万向节死锁（Gimbal Lock）问题，通常采用四元数来计算旋转。但在自动驾驶领域里很少这么干，因为相机是固定在车子上，只有垂直于地面的轴（一般是Z轴）才会发生360度的旋转，根本无法引发万向节问题，总不至于用户坚持在翻车的阶段仍旧保持自动驾驶这个诡异的需求。所以在自动驾驶目标检测或车道线检测里的代码里通常就是矩阵形式，SLAM因为还会用在AR和其它领域，相机不是相对固定的，所以会采用四元数。 #### 3.3.2.1 旋转矩阵 旋转矩阵是一个3x3的正交矩阵，用来描述一个刚体绕着某个轴旋转的情况。旋转矩阵的每一列代表了一个坐标系中的向量，因此可以通过旋转矩阵将一个向量从一个坐标系转换到另一个坐标系。
旋转矩阵的几何推导，在第2节中已经写过了。这部分主要从其他方面对其进行推导。
假设空间中有一向量a，在坐标系1中的坐标为a1，坐标系1旋转一个角度后的坐标系为2，假设在2坐标系中向量a坐标为a2，则有：
$a_2 = R^2_1 a_1$
向量在坐标系中的表示是基的线性组合，即：
$a = \left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{c} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]$
旋转过程中，向量不变，则有：
$a = \left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{c} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right]$
在两边同时乘以坐标系1基的转置，则有：
$a = \left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{lll} \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \stackrel{\text { def }}{=} \boldsymbol{R} \boldsymbol{a}^{\prime}$
可以看出，旋转矩阵为两个坐标系的内积，由于基向量的长度为1，所以实际上是各基向量夹角的余弦值。所以这个矩阵也叫方向余弦矩阵(Direction Cosine Matrix)。
关于旋转矩阵的自由度：其一共有9个数，每个列向量模为1，一共有3个自由度。两两垂直的约束，有2个，另外遵循右手系或者左手系，有一个约束。所以旋转矩阵自由度为9 - 3 - 2 -1 = 3。即旋转矩阵自由度为3。 #### 3.3.2.2 四元数 如果我们由两个复数A = a + b i 和 C = c + d i ，那么构造Q = A + C j并定义k ≜ i j k 为四元数空间H中的一个数。
$\mathrm{Q}=\mathrm{a}+\mathrm{bi}+\mathrm{cj}+\mathrm{dk} \in \mathbb{H}\\$
其中{ a , b , c , d } ∈ R 且{ i , j , k }是如下定义的三个虚单位数。
$\mathrm{i}^{2}=\mathrm{j}^{2}=\mathrm{k}^{2}=\mathrm{ijk}=-1 \\$
从其中我们可以推导
$\mathrm{ij}=-\mathrm{ji}=\mathrm{k}, \quad \mathrm{jk}=-\mathrm{kj}=\mathrm{i}, \quad \mathrm{ki}=-\mathrm{ik}=\mathrm{j}$
四元数：从上式可以看到在四元数定义中嵌入复数，从而可以将实数和虚数嵌入，即实数，虚数和复数确实是四元数。
$\mathrm{Q}=\mathrm{a} \in \mathbb{R} \subset \mathbb{H}, \quad \mathrm{Q}=\mathrm{bi} \in \mathbb{I} \subset \mathbb{H}, \quad \mathrm{Q}=\mathrm{a}+\mathrm{bi} \in \mathbb{Z} \subset \mathbb{H}\\$
同样，为了完整起见，我们可以在三维中定义数字H的虚子空间。我们将其称为 pure quaternions（纯四元数），并且，令H p = I m ( H ) 为纯四元数空间。
$\mathrm{Q}=\mathrm{bi}+\mathrm{cj}+\mathrm{dk} \in \mathbb{H}_{\mathrm{p}} \subset \mathbb{H} \text {. }\\$
四元数表示3D空间中的旋转:
$\mathbf{x}^{\prime}=\mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*}$
其中q*为q的共轭，需要注意，只有单位四元数才表示旋转。
旋转矩阵用9个量描述3自由度的旋转，具有冗余性。后面提到的欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述旋转的方式。而四元数(Quaternion)，它既是紧凑的，也没有奇异性。如果说缺点，四元数不够直观，其运算稍复杂些。
四元数与旋转矩阵的对比：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1689994670732-f691832c-ea00-49e7-948b-d88d7c199e88.png#averageHue=%23f7f6f5&clientId=ud0c2875e-b0fd-4&from=paste&height=626&id=ubc1c0e9c&originHeight=782&originWidth=700&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=110567&status=done&style=none&taskId=u008ebcd6-d2d7-425e-aeba-b7e5a4a357c&title=&width=560)
实+虚的表示法对于我们的目的并不总是方便的。假设使用了代数，则四元数可以表示为标量+向量的形式。
$\mathrm{Q}=\mathrm{q}_{\mathrm{w}}+\mathrm{q}_{\mathrm{x}} \mathrm{i}+\mathrm{q}_{\mathrm{y}} \mathrm{j}+\mathrm{q}_{\mathrm{z}} \mathrm{k} \Leftrightarrow \mathrm{Q}=\mathrm{q}_{\mathrm{w}}+\mathbf{q}_{\mathrm{v}} \\$
其中qw是实部或标量，而qv = qxi + qyj + qzk = ( qx , qy , qz ) 是 虚部或向量部分。它也可以被定义为标量-向量的有序对。
$\mathrm{Q}=\left\langle\mathrm{q}_{\mathrm{w}}, \mathbf{q}_{\mathrm{v}}\right\rangle .$
我们大多数时候将一个四元素Q表达为一个四维向量q：
$\mathbf{q} \triangleq\left[\begin{array}{l} q_{w} \\ q_{v} \end{array}\right]=\left[\begin{array}{l} q_{w} \\ q_{x} \\ q_{y} \\ q_{z} \end{array}\right]$
它允许我们对涉及到四元数的操作使用矩阵代数。在特定的场合，我们可能会通过滥用符号“ =”来允许自己混合符号，典型的例子是实四元数和纯四元数。
$一般: \mathbf{q}=\mathrm{q}_{\mathrm{w}}+\mathbf{q}_{\mathrm{v}}=\left[\begin{array}{l}\mathbf{q}_{\mathrm{w}} \\ \mathbf{q}_{\mathrm{v}}\end{array}\right] \in \mathbb{H} , 实四元数: \mathbf{q}_{\mathrm{w}}=\left[\begin{array}{l}\mathbf{q}_{\mathrm{w}} \\ \mathbf{0}_{\mathrm{v}}\end{array}\right] \in \mathbb{R} , 纯四元数: \mathbf{q}_{\mathrm{v}}=\left[\begin{array}{c}0 \\ \mathbf{q}_{\mathrm{v}}\end{array}\right] \in \mathbb{H}_{\mathrm{p}} .$
四元数标量与向量顺序和ijk方向的定义没有固定的形式。多种选择导致四元数有12种不同的组合。历史的发展使某些约定胜于其他约定（Chou，1992； Yazell，2009）。今天，在现有文献中，我们发现了许多四元数flavors，如Hamilton，STS【框架变换系统，俗称美国宇航局的航天飞机】，JPL【喷气推进实验室】，ISS【国际空间站】，ESA【欧洲航天局】，工程学，机器人学以及可能有更多的教派。这些形式中的大多数可能相同，其他的则不同，但是这个事实很少被明确指出，并且许多工作对上述四种选择都缺乏足够的四元数描述。
Hamilton和JPL是两种最常用的约定，也是记录最好的约定。表2总结了它们的特性。 JPL主要用于航空领域，而Hamilton则在其他工程领域（例如机器人技术）中更为常见。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1689995651556-4264a6d5-8596-40d6-b1b1-b7dc1d9e0d5e.png#averageHue=%23f4f2f1&clientId=ud0c2875e-b0fd-4&from=paste&height=205&id=u3864bd4c&originHeight=256&originWidth=596&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=35361&status=done&style=none&taskId=u0d98e169-420d-408b-8cd5-a5c6d46f656&title=&width=476.8)
本教程的四元数指Hamilton，这也是很多C++库如Eigen，ceres中使用的四元数定义。 #### 3.3.2.3 欧拉角 无论是旋转矩阵还是四元数，它们虽然能描述旋转，但对人类来说是非常不直观的。当我们看到一个旋转矩阵或旋转向量时，很难想象出这个旋转究竟是什么样的。当它们变换时，我们也不知道物体是在向哪个方向转动。而欧拉角则提供了一种非常直观的方式来描述旋转。
它使用了3个分离的转角，把一个旋转分解成3次绕不同轴的旋转。而人类很容易理解绕单个轴旋转的过程。在特定领域内，欧拉角通常有统一的定义方式。你或许在航空、航模中听说过“俯仰角”“偏航角”这些词。欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”(yaw-pitch-roll)3个角度来描述一个旋转。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1690014993100-26c84fd9-605c-4679-bd0f-9150176fadc6.png#averageHue=%23f8f8f8&clientId=ud0c2875e-b0fd-4&from=paste&height=235&id=u77b38fbe&originHeight=294&originWidth=912&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=84982&status=done&style=none&taskId=ufcb292e1-9e32-4acb-b311-88b2afec860&title=&width=729.6)
欧拉角可以细分为欧拉角(Euler-angles)和泰特布莱恩角(Tait-Bryan-angles)。欧拉角的旋转轴选取顺序有(x,y,x),(x,z,x),(y,x,y),(y,z,y),(z,x,z),(z,y,z)这6种，选取顺序是a,b,a。泰特布莱恩角的旋转轴选取有(x,y,z),(x,z,y),(y,x,z),(y,z,x),(z,x,y),(z,y,x)这6种，也就是遍历笛卡尔坐标系的三轴，比如我们最常见到的Roll-Pitch-Yw角就是其中(x,y,z)的情况。
欧拉角是在空间中用最直观的方式和最少的参数表示任意方向的通用方法，用它们表示方向没有计算要求和容量需求的区别。图中演示的就是用(z,x,z')方法表示方向的过程。分别绕着原坐标z轴（蓝），一次旋转以后的x轴（绿）以及两次旋转以后的z轴（红）旋转，最终产生的红色坐标系即表示出目标方向。
![](https://cdn.nlark.com/yuque/0/2023/webp/1782698/1689557362029-c89d9485-5d12-4191-95d6-4640a55e2d56.webp#clientId=u96d715ce-fc82-4&from=paste&height=267&id=bcNxp&originHeight=348&originWidth=368&originalType=url&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&taskId=u18bca8d7-dd7e-43f9-9741-64ed890b59c&title=&width=282)
欧拉角对应的旋转矩阵计算方式很简单，就是把三个连续的旋转对应的旋转矩阵右乘在一起，就得到最终的旋转矩阵M。
$\begin{array}{l} M=\operatorname{Rot}(z, \gamma) \cdot \operatorname{Rot}(x, \beta) \cdot \operatorname{Rot}(z, \alpha) \\ =\left(\begin{array}{ccc} \cos \gamma & -\sin \gamma & 0 \\ \sin \gamma & \cos \gamma & 0 \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \beta & -\sin \beta \\ 0 & \sin \beta & \cos \beta \end{array}\right)\left(\begin{array}{ccc} \cos \alpha & -\sin \alpha & 0 \\ \sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 1 \end{array}\right) \\ M=\left(\begin{array}{ccc} \cos \alpha \cos \gamma-\sin \alpha \cos \beta \sin \gamma & -\sin \alpha \cos \gamma-\cos \alpha \cos \beta \sin \gamma & \sin \beta \sin \gamma \\ \cos \alpha \sin \gamma+\sin \alpha \cos \beta \cos \gamma & -\sin \alpha \sin \gamma+\cos \alpha \cos \beta \cos \gamma & -\sin \beta \cos \gamma \\ \sin \alpha \sin \beta & \cos \alpha \sin \beta & \cos \beta \end{array}\right) \end{array}$
当然，这是欧拉角 (z,x,z) 的情况，大家如果感兴趣，可以去画画看其他的欧拉角和泰特布莱恩角，也可以试着去推一推公式，我们可以看出，无论哪种表示方式，记录这样一个变换，至少需要三个角的sine和cos值，也就是一共存储6个单位数据。
欧拉角有两个问题，一个是非一一对应的问题，另外一个是存在万向锁问题。
首先是非一一对应问题。用欧拉角表示方向（或者说，方向变换）只需要用到三个参数，即三个旋转角度（因为坐标轴是旋转轴，所以不用增加特别的参数描述旋转轴），这样做有一个非常大的优点，就是表述清晰易懂。但随之而来的有一个很重要的问题，就是Ambiguity（歧义性），简单来说就是，当给定了欧拉角以后，我们很容易找到欧拉角表述的方向，但是当我们获得了一个方向以后，却不一定能反推回目标欧拉角，原因很简单，三维空间中的任意一个方向都可以通过至少两种不同欧拉角表示。
第二个问题是万向锁，如果第一次旋转角度为90°的话，会出现旋转丢掉一个自由度的现象，这样在丢失的自由度上就永远无法得到调节，即在丢失自由度上被锁死。具体细节可看参考目录3（[3.欧拉角万向节死锁](https://zhuanlan.zhihu.com/p/344050856)）。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1690015075569-2c35673d-adb9-463a-a783-72230a0061d5.png#averageHue=%23f6f6f6&clientId=ud0c2875e-b0fd-4&from=paste&height=199&id=ue4da259f&originHeight=249&originWidth=878&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=83074&status=done&style=none&taskId=u1ca5e646-c132-4228-b032-1a5ccfb8002&title=&width=702.4) #### 3.3.2.4 旋转向量 回到开头部分，矩阵表示旋转至少有以下两个缺点：
1.S0(3)的旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的。同理，变换矩阵用16个量表达了6自由度的变换。那么，是否有更紧凑的表示呢？
2.旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。变换矩阵也是如此。当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变得更困难。
因此，我们希望有一种方式能够紧凑地描述旋转和平移。例如，用一个三维向量表达旋转，用一个六维向量表达变换，可行吗？事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量(或轴角/角轴，Axis-Angle),只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。
任意旋转都可以用一个旋转轴和一个旋转角来刻画。对应于一个三维向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量(或轴角/角轴，Axis-Angle)，该三维向量即可描述旋转。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1688917447482-8586dff2-8b5b-4f05-9c9d-79e84a7f68e9.png#averageHue=%23f5f5f5&clientId=u7e48237d-7f4e-4&from=paste&height=226&id=ucbfb5f8f&originHeight=385&originWidth=837&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=36698&status=done&style=none&taskId=u2f998267-c7d5-4bd6-8b94-04619948116&title=&width=490.6000061035156)
如图所示，u为单位向量，为旋转的轴，该次旋转一共旋转了Φ°，则轴角为Φn。从轴角到旋转矩阵，可以用罗德里格斯公式来转换：
$\begin{array}{l} \boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{\mathrm{T}}+\sin \theta \boldsymbol{n}^{\wedge}\\ \end{array}$
符号^是向量到反对称矩阵的转换符。反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角0，取两边的迹，有
$\begin{aligned} \operatorname{tr}(\boldsymbol{R}) & =\cos \theta \operatorname{tr}(\boldsymbol{I})+(1-\cos \theta) \operatorname{tr}\left(\boldsymbol{n} \boldsymbol{n}^{\mathrm{T}}\right)+\sin \theta \operatorname{tr}\left(\boldsymbol{n}^{\wedge}\right) \\ & =3 \cos \theta+(1-\cos \theta) \\ & =1+2 \cos \theta \end{aligned}\\$
所以有：
$\theta=\arccos \frac{\operatorname{tr}(\boldsymbol{R})-1}{2}$ ## 本章小结 本节主要介绍三维空间中的刚体运动如何描述，及旋转的四种描述形式。可以看到，对于同一种量，会有不同描述形式，这个过程为参数化过程，而不同参数化形式对问题会有不同的求解性能。四种旋转的描述可以相互间转换，转换详情见参考目录6（[6.刚体运动中的坐标变换-旋转矩阵、旋转向量、欧拉角及四元数](https://blog.csdn.net/hu_hao/article/details/117197727?spm=1001.2014.3001.5502)），感兴趣的同学可以自行查阅。 ## 本章思考 1.验证四元数旋转某个点后，结果是一个虚四元数（实部为零），所以仍然对应到一个三维
空间点。
2.一般线性方程Ac=b有哪几种做法？你能在Eigen中实现吗？ ## 本章练习-以H3.6M三维人体姿态数据集为例进行说明 Hunman3.6M是一个用于 3D 人体位姿估计研究的大型公开数据集，是目前基于多视图的 3D 人体位姿研究最为重要的一个数据集。该数据集由 4 台数码相机收集的 360 万个不同的人体姿势组成，因此得名H3.6M。在 [paperswithcode](https://paperswithcode.com/sota/3d-human-pose-estimation-on-human36m) 网站中可以看到在此数据集上提出的各类 [SOTA](https://so.csdn.net/so/search?q=SOTA&spm=1001.2101.3001.7020) 算法及模型。
选取这个数据集的原因：**相机内参、外参均已给定**，可以通过旋转矩阵和平移向量的修改来改变数据。这样有两个用处：1、可以以此测试算法的鲁棒性。2、可以简单实现数据的扩增来提升算法精度。
回顾一下相机内参外参： 1. 从相机坐标系转换到像素坐标系中，相机内参的作用。 2. 从世界坐标系转换到相机坐标系中，相机外参的作用。 以下是H3.6M数据集给出的内外参代码段，在这段代码中读者可以看到所学内容是如何在代码中呈现出来的： ```python h36m_cameras_intrinsic_params = [ { 'id': '54138969', 'center': [512.54150390625, 515.4514770507812], 'focal_length': [1145.0494384765625, 1143.7811279296875], 'radial_distortion': [-0.20709891617298126, 0.24777518212795258, -0.0030751503072679043], 'tangential_distortion': [-0.0009756988729350269, -0.00142447161488235], 'res_w': 1000, 'res_h': 1002, 'azimuth': 70, }, { 'id': '55011271', 'center': [508.8486328125, 508.0649108886719], 'focal_length': [1149.6756591796875, 1147.5916748046875], 'radial_distortion': [-0.1942136287689209, 0.2404085397720337, 0.006819975562393665], 'tangential_distortion': [-0.0016190266469493508, -0.0027408944442868233], 'res_w': 1000, 'res_h': 1000, 'azimuth': -70, }, { 'id': '58860488', 'center': [519.8158569335938, 501.40264892578125], 'focal_length': [1149.1407470703125, 1148.7989501953125], 'radial_distortion': [-0.2083381861448288, 0.25548800826072693, -0.0024604974314570427], 'tangential_distortion': [0.0014843869721516967, -0.0007599993259645998], 'res_w': 1000, 'res_h': 1000, 'azimuth': 110, }, { 'id': '60457274', 'center': [514.9682006835938, 501.88201904296875], 'focal_length': [1145.5113525390625, 1144.77392578125], 'radial_distortion': [-0.198384091258049, 0.21832367777824402, -0.008947807364165783], 'tangential_distortion': [-0.0005872055771760643, -0.0018133620033040643], 'res_w': 1000, 'res_h': 1002, 'azimuth': -110, }, ] h36m_cameras_extrinsic_params = { 'S1': [ { 'orientation': [0.1407056450843811, -0.1500701755285263, -0.755240797996521, 0.6223280429840088], 'translation': [1841.1070556640625, 4955.28466796875, 1563.4454345703125], }, { 'orientation': [0.6157187819480896, -0.764836311340332, -0.14833825826644897, 0.11794740706682205], 'translation': [1761.278564453125, -5078.0068359375, 1606.2650146484375], }, { 'orientation': [0.14651472866535187, -0.14647851884365082, 0.7653023600578308, -0.6094175577163696], 'translation': [-1846.7777099609375, 5215.04638671875, 1491.972412109375], }, { 'orientation': [0.5834008455276489, -0.7853162288665771, 0.14548823237419128, -0.14749594032764435], 'translation': [-1794.7896728515625, -3722.698974609375, 1574.8927001953125], }, ], 'S2': [ {}, {}, {}, {}, ], 'S3': [ {}, {}, {}, {}, ], 'S4': [ {}, {}, {}, {}, ], 'S5': [ { 'orientation': [-0.42792025, 0.64015153, -0.5395463, 0.34055846], 'translation': [2097.3916015625, 4880.94482421875, 1605.732421875], }, { 'orientation': [0.43592995, 0.64571192, -0.51878033, -0.35197751], 'translation': [2031.7008056640625, -5167.93310546875, 1612.923095703125], }, { 'orientation': [0.64472645, -0.43757454, 0.32732173, -0.53452485], 'translation': [-1620.5948486328125, 5171.65869140625, 1496.43701171875], }, { 'orientation': [0.65817815, 0.45242671, -0.30823131, -0.51682207], 'translation': [-1637.1737060546875, -3867.3173828125, 1547.033203125], }, ], 'S6': [ { 'orientation': [0.1337897777557373, -0.15692396461963654, -0.7571090459823608, 0.6198879480361938], 'translation': [1935.4517822265625, 4950.24560546875, 1618.0838623046875], }, { 'orientation': [0.6147197484970093, -0.7628812789916992, -0.16174767911434174, 0.11819244921207428], 'translation': [1969.803955078125, -5128.73876953125, 1632.77880859375], }, { 'orientation': [0.1529948115348816, -0.13529130816459656, 0.7646096348762512, -0.6112781167030334], 'translation': [-1769.596435546875, 5185.361328125, 1476.993408203125], }, { 'orientation': [0.5916101336479187, -0.7804774045944214, 0.12832270562648773, -0.1561593860387802], 'translation': [-1721.668701171875, -3884.13134765625, 1540.4879150390625], }, ], 'S7': [ { 'orientation': [0.1435241848230362, -0.1631336808204651, -0.7548328638076782, 0.6188824772834778], 'translation': [1974.512939453125, 4926.3544921875, 1597.8326416015625], }, { 'orientation': [0.6141672730445862, -0.7638262510299683, -0.1596645563840866, 0.1177929937839508], 'translation': [1937.0584716796875, -5119.7900390625, 1631.5665283203125], }, { 'orientation': [0.14550060033798218, -0.12874816358089447, 0.7660516500473022, -0.6127139329910278], 'translation': [-1741.8111572265625, 5208.24951171875, 1464.8245849609375], }, { 'orientation': [0.5912848114967346, -0.7821764349937439, 0.12445473670959473, -0.15196487307548523], 'translation': [-1734.7105712890625, -3832.42138671875, 1548.5830078125], }, ], 'S8': [ { 'orientation': [0.14110587537288666, -0.15589867532253265, -0.7561917304992676, 0.619644045829773], 'translation': [2150.65185546875, 4896.1611328125, 1611.9046630859375], }, { 'orientation': [0.6169601678848267, -0.7647668123245239, -0.14846350252628326, 0.11158157885074615], 'translation': [2219.965576171875, -5148.453125, 1613.0440673828125], }, { 'orientation': [0.1471444070339203, -0.13377119600772858, 0.7670128345489502, -0.6100369691848755], 'translation': [-1571.2215576171875, 5137.0185546875, 1498.1761474609375], }, { 'orientation': [0.5927824378013611, -0.7825870513916016, 0.12147816270589828, -0.14631995558738708], 'translation': [-1476.913330078125, -3896.7412109375, 1547.97216796875], }, ], 'S9': [ { 'orientation': [0.15540587902069092, -0.15548215806484222, -0.7532095313072205, 0.6199594736099243], 'translation': [2044.45849609375, 4935.1171875, 1481.2275390625], }, { 'orientation': [0.618784487247467, -0.7634735107421875, -0.14132238924503326, 0.11933968216180801], 'translation': [1990.959716796875, -5123.810546875, 1568.8048095703125], }, { 'orientation': [0.13357827067375183, -0.1367100477218628, 0.7689454555511475, -0.6100738644599915], 'translation': [-1670.9921875, 5211.98583984375, 1528.387939453125], }, { 'orientation': [0.5879399180412292, -0.7823407053947449, 0.1427614390850067, -0.14794869720935822], 'translation': [-1696.04345703125, -3827.099853515625, 1591.4127197265625], }, ], 'S11': [ { 'orientation': [0.15232472121715546, -0.15442320704460144, -0.7547563314437866, 0.6191070079803467], 'translation': [2098.440185546875, 4926.5546875, 1500.278564453125], }, { 'orientation': [0.6189449429512024, -0.7600917220115662, -0.15300633013248444, 0.1255258321762085], 'translation': [2083.182373046875, -4912.1728515625, 1561.07861328125], }, { 'orientation': [0.14943228662014008, -0.15650227665901184, 0.7681233882904053, -0.6026304364204407], 'translation': [-1609.8153076171875, 5177.3359375, 1537.896728515625], }, { 'orientation': [0.5894251465797424, -0.7818877100944519, 0.13991211354732513, -0.14715361595153809], 'translation': [-1590.738037109375, -3854.1689453125, 1578.017578125], }, ], } ``` 有了上面的信息，我们可以使用python里的numpy和scipy库进行姿态变换。首先将四元数转换为旋转矩阵，之后定义一个新的旋转矩阵并进行矩阵点乘；平移向量相加即可实现变换。 ```python import numpy as np from scipy.spatial.transform import Rotation # 定义姿态变换参数: 在这里定义姿态变换参数，例如旋转矩阵R或欧拉角yaw/pitch/roll等 # 以下是一个简单的旋转矩阵示例 # 对每个人体姿势数据进行姿态变换，以单个为例 q0, qx, qy, qz = [0.5834008455276489, -0.7853162288665771, 0.14548823237419128, -0.14749594032764435] R_input = Rotation.from_quat([qx, qy, qz, q0]).as_matrix() R_new = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) R_output = np.dot(R_new, R_input) q = Rotation.from_matrix(R_output).as_quat() # 平移向量的修改 translation = [2044.45849609375, 4935.1171875, 1481.2275390625] translation_new = np.array([0, 0, 1]) translation += translation_new #输出修改后的数据： print(q) print(translation) ``` ## 参考 0.《视觉SLAM十四讲》
[1.旋转的左乘与右乘](https://zhuanlan.zhihu.com/p/128155013)
[2.如何通俗地解释欧拉角？之后为何要引入四元数？]()
[3.欧拉角万向节死锁](https://zhuanlan.zhihu.com/p/344050856)
[4.第四讲：李群和李代数](https://zhuanlan.zhihu.com/p/33156814)
[5.文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第1章](https://blog.csdn.net/sunqin_csdn/article/details/108560582)
[6.刚体运动中的坐标变换-旋转矩阵、旋转向量、欧拉角及四元数](https://blog.csdn.net/hu_hao/article/details/117197727?spm=1001.2014.3001.5502)
[7.如何通俗易懂的解释自动驾驶中的BEV和SLAM？](https://zhuanlan.zhihu.com/p/646310465) ================================================ FILE: docs/chapter4/chapter4.md ================================================ # **4.也见森林：视觉SLAM简介** 构成我们学习最大障碍的是已知的东西，而不是未知的东西。——贝尔纳 --- | **本章主要内容**
1.什么是视觉SLAM
2.视觉SLAM的主流框架及分类 | | --- | 前三章都在讲解相机，刚体运动相关内容，属于一个铺垫，那么什么是SLAM呢，在本章进行介绍。 ## **4.1 SLAM** SLAM为Simultaneous Localization and Mapping的缩写，意为同时定位与建图。这有啥用呢，想象一下到了一个陌生环境中，比如一下被扔到阿尔卑斯山某个山谷中，你需要找到出去的路，给自己打完气后，就出发了，往前面走啊走，走啊走，你突然发现目前这个场景好熟悉，是不是哪里见过。不太肯定，也没心思去管这个了，继续走啊走，走啊走，你才大呼一声，这特么不是我刚才踩断的那截树根么。你站在这个还很新的断面接口前方，双方仿佛大眼瞪小眼相互看着，有着些许的尴尬，这时候你想，要是能把走过的地方都记录下来，并从中提取出方向信息，确定自己的位置，那该多好啊。
是的，SLAM就是干这个事的。利用载体上的各种传感器，记录环境的同时估计载体的空间姿态，最后生成一张可用于地位的地图，就是SLAM的使命。
![](https://cdn.nlark.com/yuque/0/2023/gif/1782698/1683986340140-48dfd9b5-f1f1-4693-adcf-87e5ca65c933.gif#averageHue=%231f2636&id=qPEET&originHeight=240&originWidth=320&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=)
这里的载体，指能在空间中运动的载体，移动机器人，车辆，手持设备等都可以算是移动载体。而要达到对环境及自身运动的估计，则需要传感器。传感器目前主要分为两类，测量值依赖外界环境的，与测量值依赖自身运动情况的。依赖外部环境的传感器如相机，激光雷达。相机需要收集外界环境光线，而雷达需要接收外界物体表面反射的激光。依赖自身运动的则如一些编码器与imu，imu测量得到的加速度与载体运动情况有关。载体与传感器的关系有如身体与眼睛的关系一样，载体为传感器提供了搭载的平台，而传感器则是载体感知世界的来源，计算机模仿大脑处理传感器信息，经过一系列算法，把传感器采集的数据不断拼接为环境的模型，并估计出自身的位姿。 ## **4.2 视觉SLAM** 视觉SLAM，也称为VSLAM，指利用视觉传感器来进行建图与定位的技术。常用的视觉传感器就是相机，而相机有很多种，如单目相机，双目相机，深度相机等等。这类相机统称为视觉传感器。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986340610-386f7fd1-8350-4ae0-a4f0-cf02626650d9.png#averageHue=%23bfbfbf&id=eLkwV&originHeight=399&originWidth=774&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=)
直白来说，视觉SLAM，就是要用图像这个信息，去认识世界，定位自己。 ## **4.3 视觉SLAM主流框架** ### **4.3.1 主流框架** 经过多年的发展，目前VSLAM框架已趋近成熟，总体说来，分为前端估计，后端优化，回环检测，地图构建这四个模块。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986341029-ab1a3a21-9be7-4df0-97a4-6b539fc24849.png#averageHue=%23caccc6&id=RA8aS&originHeight=449&originWidth=1114&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=)
首先接收传感器数据后，看数据情况会有一个预处理过程。之后前端会对传感器数据进行特征提取及特征追踪，这样可以得到前端的一个里程计估计及短中期的一个数据关联。前端信息经过一定筛选，留下一些有代表性的信息帧，传递给后端，同时前端特征也会与历史帧进行匹配，检测是否有回到之前到过的地方，如果有，也会把这个信息传递给后端，后端使用这个回环检测信息及全部关键帧信息进行全局的优化，得到最优的观测量及姿态值的估计，最后把姿态及特征保存为地图。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986341446-f17a4890-5a09-4d89-857a-75b75449b67e.png#averageHue=%23f3f4d8&id=ccZ6I&originHeight=413&originWidth=669&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=)
总结来说，需要一个模块处理原始传感器数据，然后数据给到前端模块进行粗加工，提取其中的特征来短时间估计当前移动情况及特征的追踪情况（前端），然后把所有这些信息给到后端，作为优化的初始值。同时前端特征会与历史观测进行匹配（回环检测），并把回环检测结果发送给后端。后端把所有信息进行最优化估计（最大后验估计），得到最优的地图，最后生成一张可用来定位或者导航的地图（建图）。 ### **4.3.2 视觉SLAM分类** 在以上框架中，按照前端使用不同的特征，可以把当前视觉slam分为直接法，间接法，及联合使用这两类的混合法。直接法使用图像的灰度梯度信息，按照使用的图像范围，分为稠密，半稠密，稀疏。而间接法，则从图像中提取一些特征点，提取完特征点后便舍弃掉整幅图像，所以间接法也叫基于特征的方法。而接着按后端中使用的优化技术方案，又可以把上述几个大类接着细分为基于滤波的方法，基于非线性优化的方法。
同时，也有按照算法适配的相机类型，把VSLAM算法分为单目算法，双目算法及RGB-D算法的。下图是几个类别的VSLAM代表算法及出现的时间，可以看出最早的VSLAM算法是基于特征的单目算法，之后出现了混合算法，再最新出现了直接法。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986341855-c1ab9e71-3243-4cf5-a625-e3eb5fb30882.png#averageHue=%23b6b590&id=Nbxal&originHeight=429&originWidth=907&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=)
而对于后端优化，则是最先出现的滤波技术，然后出现了非线性优化技术。滤波计算量小，速度快，但结果精度差于非线性优化。非线性优化分为滑窗优化及全局优化，计算量大，使用的观测更多，速度慢一些，但目前取得了最高精度。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683986342240-10d242e7-6971-4715-87c4-d3cde59d8b9f.png#averageHue=%23f8f1eb&id=PcPVx&originHeight=329&originWidth=589&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=)
目前直接法的代表算法，有LSD，和慕尼黑工业大学的DSO
间接法代表算法，有MonoSLAM，PTAM，ORB_SLAM
混合法代表算法，有SVO ### **4.3.3 单目VSLAM的一个工作示例** 简单来说，单目VSLAM的工作流程为提取特征点->特征点追踪->系统初始化->初始位姿估计->关键帧判断->局部BA->回环检测->全局BA。这里我们以经典的ORB_SLAM为例，讲一个单目VSLAM是如何工作的。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1693616383576-afd6cccf-c7a1-4e4b-87fc-76b3ed0bf71d.png#averageHue=%23a6927a&clientId=u349385ce-be18-4&from=paste&height=438&id=u2e773e45&originHeight=579&originWidth=723&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=183964&status=done&style=none&taskId=u16e83959-b50f-4950-87f1-bd4d01c9c7e&title=&width=547.4000244140625)
上图是ORB_SLAM（ORB_SLAM1）的系统总览。整个系统分为追踪（Tracking）,局部建图（Local Mapping）和回环检测（Loop Closing）三大线程，并维护地图及视觉词典两大数据结构。
**1.特征追踪**
当接收到每一帧图像数据时，追踪线程首先在图像上提取角点特征（ORB特征）。在系统刚启动时，单目VSLAM系统由于缺乏尺度信息，需要先利用对极几何进行初始化。这个时候需要相机在空间中运动一小段距离以保证对极约束的有效性，ORB_SLAM在初始化中会开两个线程，对本质矩阵和单应矩阵同时进行估计，选择效果最好的模型进行初始化。
初始化完成后，系统就获得了带尺度信息的初始地图。对于后面新接收到每一帧图像，再利用特征点的匹配关系，就可以计算出当前帧的位姿。在获得当前帧位姿之后，ORB_SLAM会在局部地图中将具有共视关系帧上的特征点也投影到当前帧上，以寻找更多的匹配点，并利用新找到的匹配点再次对相机位姿进行优化，这一步称为局部地图追踪。在完成这些步骤之后，会根据当前帧相对于上一帧运动的距离及匹配到的特征点数量，判断当前帧是否有条件成为一个关键帧，如果是关键帧，则会进行局部建图，如果不是，则继续处理下一帧。
**2.局部建图**
在新生成一个关键帧之后，便会进入局部建图线程。新的关键帧的插入会使ORB_SLAM更新其“生成树”（spanning Tree）及共视图（Covisibility Graph）及本质图（Essential Graph）。这些数据结构维护关键帧之间的相对位移关系及共视关系。更新完这些数据结构后，就完成了关键帧的插入。
接下来是对不可靠地图点的剔除，即能观测到该地图点的关键帧数量少于一个阈值，这个地图点作用就不大了，此时会对其进行剔除。剔除完之后就是新地图点的生成，对于新检测到的特征点，如果和之前未匹配上的地图点产生了匹配，这个特征点就可以恢复出深度，并产生新的3D地图点。之后对新的关键帧及与其有共视关系的局部地图帧，及其它们能看到的地图点，会进行局部BA，通过新的约束关系提高局部地图精度。局部追踪和局部BA利用了中期数据关联，是ORB_SLAM精度高的一大原因。在完成局部BA后，ORB_SLAM会对冗余的关键帧（一帧上的大部分的地图点，能被其他关键帧看到）进行剔除。这种关键帧的剔除机制，在ORB_SLAM在称为适者生存机制，是其能在一个地方长期运行，而关键帧数量不至于无限制增长的关键设计。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1693617287889-9cb75199-cdec-4f2f-ba20-3c23d8b2b0d3.png#averageHue=%23faf6f6&clientId=u349385ce-be18-4&from=paste&height=452&id=u6b8f5b7b&originHeight=706&originWidth=661&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=249892&status=done&style=none&taskId=uab014820-c249-4032-822d-3298e6166b9&title=&width=422.79998779296875)
**3.回环检测**
对于每一个关键帧，在进行完局部建图后，会进入到回环检测阶段。首先ORB_SLAM会使用DBoW库检索可能的回环帧，对检测结果会进行几何一致性检验。通过几何一致性检验后，认为发生了回环，此时会计算当前帧与回环帧之间的Sim3关系，即平移，旋转加尺度。利用这个信息，建立当前帧及其共视图，与回环帧及其共视图之间的约束关系，这一步就称为回环融合。最后，对带回环约束关系的本质图进行优化，完成对整个地图的调整。如果没有发生回环，则会把当前帧的特征描述子信息加入到视觉词典中，以与后面新的关键帧做匹配。
在这整个系统中，特征追踪部分称为前端，而局部建图，回环检测部分，则称之为后端。前端主要职责是检测特征，并寻找尽可能多的数据关联。后端则是基于前端给的初始估计及数据关联进行优化，消除观测误差及累计误差，生成精度尽可能高的地图。 ## 本章小结 介绍了什么是SLAM及其主流框架，VSLAM的主要分类。 ## 本章思考 1.VSALM中间接法与直接法的主要区别在什么地方，其各自的优势是什么？
2.SLAM中前端与后端的关系是什么？ ## **参考** [1.视觉/视觉惯性SLAM最新综述：领域进展、方法分类与实验对比](https://zhuanlan.zhihu.com/p/393381346)
[2.SLAM技术综述](https://zhuanlan.zhihu.com/p/501102444)
[3.视觉SLAM:模型介绍、算法框架及应用场景](https://zhuanlan.zhihu.com/p/623111690?utm_campaign=shareopn&utm_medium=social&utm_oi=662363645720268800&utm_psn=1632301428428447744&utm_source=wechat_session) ================================================ FILE: docs/chapter5/chapter5.md ================================================ # **5.以不变应万变：前端-视觉里程计之特征点** 愚昧从来没有给人带来幸福；幸福的根源在于知识。——左拉 --- > **本章主要内容** \ > 1.局部特征 \ > 2.特征点的分类 \ > 3. 常见的特征提取算法 \ > 4. 常见的关键点检测算法 视觉里程计对应于VSLAM中的前端部分，特征追踪是视觉里程计中的一个基础和关键的任务，它可以从两张或者多张图片中识别对应相同或相似的内容。通过追踪图像中局部特征在前后帧图像中的位置，就可以进行视觉运动估计了。特征的抽取及匹配是整个VSLAM系统中最耗时的计算过程之一，快速且鲁棒的特征点检测匹配算法可以大幅提高VSLAM算法的精度。 ## 5.1 局部特征 图像的局部特征是一种用于描述图像中具有独特性、稳定性和可区分性的局部结构或纹理信息的方法，它可以有效地提高图像匹配的效果和精度。理想的局部特征就是一个点，在实际中，数字图像离散化时，最小单位是像素点，如果要定位某个像素点的话，必须分析该像素的邻域，所以任何局部特征都是隐式的包括一个空间范围。好的局部特征在图像的变换(如旋转、缩放、平移)下相对稳定，不会因为图像的变化而发生失效。由于全局特征容易受到噪声干扰，而局部特征比较稳定，常用于图像匹配。
举一个例子：想象一下我们做拼图游戏，需要把以下六个方块图案拼到下方图像对应位置，哪几个方块是最有把握的呢？
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1697277299509-b9ba76ca-d430-4c40-a3e2-914f5ac55de4.png#averageHue=%237393b4&clientId=u7d8e55e0-8eee-4&from=paste&height=287&id=ucf2645bd&originHeight=407&originWidth=554&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=903863&status=done&style=none&taskId=u97e19756-87a0-42b8-b24c-2a2e0490dc1&title=&width=390.20001220703125)
对于A和B,我们主要能看到的是大块的色块，其特征表现出一个面的结构，蓝色的天空都可以匹配上A，沿任何方向移动都可以，不存在唯一匹配的结果，对于B也是如此。这类结构作为特征来说是缺乏足够的区分度的。对于C和D，其中的线特征最为突出，在图中我们可以去找这些所有的线，但是我们也发现，沿着线的方向，其纹理也基本是一致的，我们可以找出线的约束方向，但却不好确定是在这个方向的哪个位置。最后来到EF,我们主要能看到一个角结构，相信大家很快就能找到其在图像中对应的位置，其中的角把两个方向都约束住了。而一定程度上角点可以代表这个角结构的位置。通过这个例子，我们可以发现点特征（如角点），相比于其他类型特征更具有区分度。后面我们也会讲到，点特征相比于线特征与面特征，对于视角变化及光照变化，具有更高的鲁棒性。 ## 5.2 特征点的分类 常见的特征点分为角点与斑点两类，角点检测的算法主要是定位在图像中直线交叉或者曲率变化较大的像素点，传统的方法有基于梯度和基于密度等多种方法来找角点。相比与角点检测受噪声的影响较大的缺点，斑点检测的方法更容易获得鲁棒性和稳定性的特征，因为斑点更多的是代表一个封闭的区域/圆形区域。常见的角点检测算法如Harris、Shi-Tomasi，常见的斑点检测算法如SIFT、SURF。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1697277378446-0f4d4fe8-4cd3-4ee4-bc85-ff6fa4d1b786.png#averageHue=%23ededed&clientId=u7d8e55e0-8eee-4&from=paste&height=186&id=u0e419680&originHeight=233&originWidth=660&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=616427&status=done&style=none&taskId=u47ea4a13-c5fd-4f62-b204-2ef3cb8e50e&title=&width=528)
图 5.2 角点与斑点
基于特征的图像匹配流程主要分为三个部分，特征检测、特征描述和描述子匹配。一般特征点算法流程是先进行特征检测获取特征点在图像的位置，而后对特征点的邻域进行分析提取特征描述子。而在特征追踪时会比对不同图片上的特征描述子来获取描述子的匹配对。前两个步骤统称为特征点算法，也称特征提取。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1697277481936-a54df60e-6516-4bda-aa77-378aca2d40d4.png#averageHue=%23f7f7f6&clientId=u7d8e55e0-8eee-4&from=paste&height=186&id=u341da3b2&originHeight=232&originWidth=660&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=613774&status=done&style=none&taskId=u7ed652a6-a36e-43a5-9b70-db6711d2d58&title=&width=528)
图 5.3 图像特征匹配流程
特征描述也称特征的描述子计算，通常是将特征点周边的区域的一些信息转变为一个易于识别的形式，经常表示为一个高维的向量以此来比较两个特征点是不是相似。特征描述一般有两种方法，基于梯度的方法和基于强度的方法。基于梯度的方法常见的是根据区域内的像素梯度方向来生成描述子，基于强度的方法是通过比对特定位置像素点的强弱来生成描述子，例如LBP算法通过对比邻域和中心的点的强度来生成描述子。常见的基于梯度的方法有SIFT和SURF等，基于强度的方法有LBP和BRIEF等。
需要注意的是，有的角点检测算法如Harris、FAST与Shi-Tomasi只有第一步特征检测，没有特征描述部分，这类算法一般称为角点检测算法，还不属于完整的特征点算法。接下来我们先介绍常用的完整的特征点算法，然后再介绍常用的角点检测算法。 ## **5.3 常见的特征提取算法** ### 5.3.1 SIFT SIFT，全称为尺度不变特征变换（Scale-Invariant Feature Transform），是一种经典的特征点检测算法，该方法由加拿大教授大卫.劳伊（David G.Lowe）提取，他总结了基于不变量技术的特征检测方法，并1999年正式提出了一种基于尺度空间的、对图像缩放、旋转甚至仿射变换保持不变性的图像局部特征描述算子，这种算法在2004年被加以完善申请了专利，其专利属于英属哥伦比亚大学。SIFT专利在2020年3月17日之后到期，现在只需更新cv版本即可免费使用。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682952698032-eafa47ba-052c-48bc-a37c-d990f2e0a0fa.png#averageHue=%239ba2a3&clientId=u536d1d11-a6b9-4&from=paste&height=246&id=uaf62eb2d&originHeight=307&originWidth=458&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=167803&status=done&style=none&taskId=udf8e65dc-eb27-4965-bd92-1a516429083&title=&width=366.4)
SIFT算法不仅具有尺度不变性，且在光照变化，图像旋转，视角变化中均有较好的不变性。要说缺点，就是SIFT运算复杂度太高，很耗时。
SIFT算法的实质可以归为在不同尺度空间上查找特征，点（关键点）的问题。其包括特征点检测，特征点描述子计算及特征点匹配算法三块。为了使主题更集中，这里主要介绍特征点检测及描述子计算两部分。匹配算法主要是计算描述子之间的距离，就不再单独介绍了。
首先要知道，SIFT的本质是在不同尺度空间上寻找极值点。这里的尺度空间，模拟的是人眼镜观测事物的一个特点，即近大远小。近处的物体看起来大且清晰，远处的物体，看起来小且模糊。要把事物这种与观测位置有关的成像特点表达出来，常使用高斯图像金字塔来实现。
先来一个简述，看看SIFT干了哪些事情，它首先从搭建图像金字塔开始，计算高斯差分图像金字塔。然后在同层及相邻层之间计算初始极值点。再使用泰勒展开计算精确极值点，其中会使用一些方法过滤掉低反应度的点及边缘点。最后使用特征点附近的梯度直方图计算主方向，并利用4x4个小方格中的8个主方向，形成128维的描述子。
下面就详细介绍这几个步骤：
**1.构建高斯差分金字塔**
图像的尺度空间函数，定义为原始图像I(x,y)与一个可变尺度的2维高斯函数G(x,y,σ)做卷积运算：
$\begin{array}{c} L(x, y, \sigma)=G(x, y, \sigma) * I(x, y) \\ G\left(x_{i}, y_{i}, \sigma\right)=\frac{1}{2 \pi \sigma^{2}} \exp \left(-\frac{\left(x-x_{i}\right)^{2}+\left(y-y_{i}\right)^{2}}{2 \sigma^{2}}\right) \end{array}$
其中σ大小决定图像的平滑程度，大尺度对应图像的概貌特征，小尺度对应图像的细节特征。大的σ值对应粗糙尺度(低分辨率)，反之，对应精细尺度(高分辨率)。
**1.1构建高斯图像金字塔：**
构建金字塔的过程主要分为两步，降采样与高斯模糊，其中高斯模糊利用高斯核来进行卷积运算，这里提一点，高斯核是线性空间中唯一可以产生多尺度空间的核。
降采样：下一组（octave）图像的长宽，为上一组图像长宽的一半，实现方式为行列间隔一行采样。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682956374718-713e4ac9-eefd-4e3a-9a7e-8fd62380495d.png#averageHue=%23e8e8e8&clientId=u536d1d11-a6b9-4&from=paste&height=198&id=u3f516226&originHeight=247&originWidth=315&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=40847&status=done&style=none&taskId=u39e93f41-fcb9-44bb-9028-1b3008c96d1&title=&width=252)
高斯卷积：
根据3σ原则，使用NxN的模板在图像每一个像素点处操作，其中N=[（6σ+1）]且向上取最邻近奇数。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682956511298-06555c4e-8002-46de-b193-2770258623c8.png#averageHue=%23e4e3e0&clientId=u536d1d11-a6b9-4&from=paste&height=355&id=u92435a0b&originHeight=623&originWidth=918&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=286862&status=done&style=none&taskId=u1a5a770d-a998-42fd-a8c7-25fed037d74&title=&width=522.4000244140625)
上面这样直接与图像卷积，速度比较慢，同时图像边缘信息也会损失严重。后来不知哪位学者发现，可以使用分离的高斯卷积（即先用1xN的模板沿着X方向对图像卷积一次，然后用Nx1的模板沿着Y方向对图像再卷积一次，其中N=[（6σ+1）]且向上取最邻近奇数），这样既省时也减小了直接卷积对图像边缘信息的严重损失。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682956554444-0d26fbb7-c994-42de-9318-4f5c861cecfc.png#averageHue=%23f3f1f1&clientId=u536d1d11-a6b9-4&from=paste&height=349&id=uac89e764&originHeight=679&originWidth=1110&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=217933&status=done&style=none&taskId=ubcfd5d9c-f347-4300-b568-67113fee3a8&title=&width=570)
这样，在不同尺寸，不同模糊程度上，对原图像进行处理，就形成了一系列的尺度空间，如下图所示。
![](https://cdn.nlark.com/yuque/0/2023/gif/1782698/1682954160273-44afce41-6c7c-4bc2-bf29-45c5bee20e12.gif#averageHue=%23585858&clientId=u536d1d11-a6b9-4&from=paste&height=424&id=uf35752ef&originHeight=480&originWidth=512&originalType=url&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&taskId=ue85bfbad-db1e-46ae-b29c-83af23b352a&title=&width=452)
**1.2高斯差分尺度空间**
为了检测到稳定的关键点，SIFT使用了高斯差分尺度空间（DOG scale-space）。利用不同尺度的高斯差分核与图像卷积生成。
$\begin{aligned} D(x, y, \sigma) & =(G(x, y, k \sigma)-G(x, y, \sigma)) * I(x, y) \\ & =L(x, y, k \sigma)-L(x, y, \sigma) \end{aligned}$
·高斯差分尺度空间的构建，由同组尺度图像，相邻层相减得到。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682956153046-9f4e31b2-3d43-4989-82f5-30308518f8d2.png#averageHue=%23878880&clientId=u536d1d11-a6b9-4&from=paste&height=272&id=S6htY&originHeight=340&originWidth=525&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=216586&status=done&style=none&taskId=u3b1ca221-04d0-43dd-87bf-0caa94fadc3&title=&width=420) **2.极值点检测**
在高斯差分层同组一层的每个像素点，及上下相邻层共26个点中，寻找极值点。判断条件为均大于周围点或者均小于周围点。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682956893464-6e35fa7a-2984-4315-946f-a22a6d6865ec.png#averageHue=%23f1f1f1&clientId=u536d1d11-a6b9-4&from=paste&height=226&id=u6f0da5ed&originHeight=283&originWidth=341&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=79030&status=done&style=none&taskId=u4096ba85-9654-4d44-8a56-09c2b76feae&title=&width=272.8)
以上方法判断的极值点是离散空间的极值点，但离散空间的极值点并不是真正的极值点，需要通过插值的方式找到准确的极值点。利用已知的离散空间点插值得到的连续空间极值点的方法叫做子像素插值。以下通过拟合三维二次函数来精确确定关键点的位置和尺度，同时去除低对比度的关键点和不稳定的边缘响应点(因为DoG算子会产生较强的边缘响应)，以增强匹配稳定性、提高抗噪声能力。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682957278962-150a78be-4094-43be-afe3-e9f3ade1613f.png#averageHue=%23f6f6f6&clientId=u536d1d11-a6b9-4&from=paste&height=209&id=u24ca855b&originHeight=261&originWidth=598&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=53314&status=done&style=none&taskId=u2c7b3320-5643-45b3-8a1a-179cd63bbd4&title=&width=478.4)
在检测到的极值点处，作三元二阶泰勒展开，计算过程如下：
$\begin{aligned} & f\left(\left[\begin{array}{l} x \\ y \\ \sigma \end{array}\right]\right)=f\left(\left[\begin{array}{l} x_0 \\ y_0 \\ \sigma_0 \end{array}\right]\right)+\left[\frac{\partial f}{\partial x}, \frac{\partial f}{\partial y}, \frac{\partial f}{\partial \sigma}\right]\left(\left[\begin{array}{l} x \\ y \\ \sigma \end{array}\right]-\left[\begin{array}{l} x_0 \\ y_0 \\ \sigma_0 \end{array}\right]\right) \\ & +\frac{1}{2}\left(\left[\begin{array}{l} x \\ y \\ \sigma \end{array}\right]-\left[\begin{array}{l} x_0 \\ y_0 \\ \sigma_0 \end{array}\right]\right)^{\mathrm{T}}\left[\begin{array}{l} \frac{\partial^2 f}{\partial x \partial x}, \frac{\partial^2 f}{\partial x \partial y}, \frac{\partial^2 f}{\partial x \partial \sigma} \\ \frac{\partial^2 f}{\partial x \partial y}, \frac{\partial^2 f}{\partial y \partial y}, \frac{\partial^2 f}{\partial y \partial \sigma} \\ \frac{\partial^2 f}{\partial x \partial \sigma}, \frac{\partial^2 f}{\partial y \partial \sigma}, \frac{\partial^2 f}{\partial \sigma \partial \sigma} \end{array}\right]\left(\left[\begin{array}{l} x \\ y \\ \sigma \end{array}\right]-\left[\begin{array}{l} x_0 \\ y_0 \\ \sigma_0 \end{array}\right]\right) \\ & \end{aligned}$
$H(X,Y)=\left[\begin{array}{c} \frac{\partial^2 f}{\partial x \partial x}, \frac{\partial^2 f}{\partial x \partial y} \\ \frac{\partial^2 f}{\partial x \partial y}, \frac{\partial^2 f}{\partial y \partial y} \end{array}\right]$
矢量形式：$f(X)=f\left(x_0\right)+\frac{\partial f^{\mathrm{T}}}{\partial X} \hat{X}+\frac{1}{2} \hat{X}^{\mathrm{T}} \frac{\partial^2 f}{\partial X^2} \hat{X}$
$f(X)$对X进行求导：
$\frac{\partial f(X)}{\partial X}=\frac{\partial f^{\mathrm{T}}}{\partial X}+\frac{1}{2}\left(\frac{\partial^2 f}{\partial X^2}+\frac{\partial f^{\mathrm{T}}}{\partial X^2}\right) \hat{X}=\frac{\partial f^{\mathrm{T}}}{\partial X}+\frac{\partial^2 f}{\partial X^2} \hat{X}$
令导数为0，可解得：
$\hat{X}=-\frac{\partial^2 f^{-1}}{\partial X^2} \frac{\partial f}{\partial X}$
舍去低对比度的点，若$|f(X)|<\frac{\mathrm{T}}{n}$，则舍去点$X$。
去除边缘效应：**本质上要去掉DoG局部曲率非常不对称的像素**，一个定义不好的高斯差分算子的极值在横跨边缘的地方有较大的主曲率，而在垂直边缘的方向有较小的主曲率。主曲率通过一个2*2的海森矩阵（Hessian Matrix）$H$求出，$D$的主曲率和$H$的特征值成正比，令$\alpha$为较大特征值，$\beta$为较小的特征值。
$H(x, y)=\left[\begin{array}{ll} D_{x x}(x, y) & D_{x y}(x, y) \\ D_{x y}(x, y) & D_{y y}(x, y) \end{array}\right]$
$\begin{aligned} & \operatorname{Tr}(H)=\alpha+\beta \quad \text { 其中: } \alpha>\beta \text { 且 } \alpha=\gamma \beta \\ & \operatorname{Det}(H)=\alpha \beta \end{aligned}$
若$\operatorname{Det}(H)<0$舍去$X$
$\frac{\operatorname{Tr}(H)}{\operatorname{Det}(H)}=\frac{(\alpha+\beta)}{\alpha \beta}=\frac{(\gamma \beta+\beta)}{\gamma \beta^2}=\frac{(\gamma+1)^2}{\gamma}$
若不满足$\frac{\operatorname{Tr}(H)}{\operatorname{Det}(H)}=\frac{(\gamma+1)^2}{\gamma}<\frac{(10.0+1)^2}{10.0}=1.21$舍去$X$ （建议$\gamma$取10.0） **3.确定特征点主方向**
统计以特征点为圆心，以该特征点所在的高斯图像的尺度的1.5倍为半径的圆内的所有的像素的梯度方向及其梯度幅值，并做1.5σ的高斯滤波(高斯加权，离圆心也就是关键点近的幅值所占权重较高)。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682957423981-872663df-39dd-478e-b530-2a1686453914.png#averageHue=%23bbeab7&clientId=u536d1d11-a6b9-4&from=paste&height=196&id=u6ad56c81&originHeight=245&originWidth=577&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=127985&status=done&style=none&taskId=ud8cb3ea3-692e-476d-a101-890300c2d6e&title=&width=461.6) 注：在最接近关键点尺度σ的高斯图像上进行统计.
关键点的方向以主方向准.
当关键点有两个方向，一个主方向，一个辅方向(梯度幅值>=80%主方向梯度幅值)，那么把这个关键点看成两个关键点，只不过这两个关键点的坐标和σ一样，只是方向不一样.此时关键点的方向还是离散的，我们需要进行抛物线插值。 **4.计算描述子**
上述过程，只是找到关键点并确定了其方向，但SIFT算法核心用途在于图像的匹配，我们需要对关键点进行数学层面的特征描述，也就是构建关键点描述符.
**1. 确定计算描述子所需的图像区域**
描述子梯度方向直方图由关键点所在尺度的高斯图像计算产生. 图像区域的半径通过下式(17)计算
$\text { radius }=\frac{3 \sigma \sqrt{2}(d+1)+1}{2}$
其中$d=4$
注：$d=4$，代表划分4*4个子块。
**2. 将坐标移至关键点方向**
关键点所在的半径区域，移至关键点方向，如图6所示：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682957511159-fba32612-1980-42f3-8fc3-ea65ea1fba7d.png#averageHue=%23eff3e7&clientId=u536d1d11-a6b9-4&from=paste&height=200&id=ua86a9bbb&originHeight=250&originWidth=506&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=107962&status=done&style=none&taskId=u8f85745f-533a-4057-a1ad-cd1186c893a&title=&width=404.8)
**3. 生成关键点描述符**
如图7所示：将区域划分为4×4的子块，对每一子块进行8个方向的直方图统计操作，获得每个方向的梯度幅值，总共可以组成128维描述向量。 ![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1682957532050-2af29452-9fad-4ddc-9289-5e1045fb165c.png#averageHue=%23ddf4d5&clientId=u536d1d11-a6b9-4&from=paste&height=400&id=u21b3b372&originHeight=500&originWidth=700&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=302590&status=done&style=none&taskId=u30feaedd-ec2c-4950-aa22-980cb7bc8a0&title=&width=560) SIFT算法首先在图像中检测出关键点，然后计算每个关键点周围的局部特征描述子。关键点的选择基于图像的极值点，同时考虑尺度空间和高斯差分图像。关键点的描述子由关键点周围的梯度方向直方图构成，该直方图具有旋转不变性和尺度不变性。
其中，第一步是构建高斯金字塔，将原始图像不断缩小，得到不同尺度的图像。第二步是构建高斯差分金字塔，从高斯金字塔中相邻两层图像相减，得到不同尺度的差分图像。第三步是在高斯差分金字塔中检测极值点，即在每个尺度的差分图像中寻找局部极值点。第四步是对极值点进行精确定位和尺度估计。第五步是确定特征方向，计算关键点周围像素点的梯度方向直方图，然后选择主方向作为关键点的描述子。最后，将关键点的描述子归一化，得到具有旋转不变性和尺度不变性的特征描述子。
总体而言，SIFT算法是一种非常有效的图像特征提取算法，能够有效地用于图像匹配、目标识别等领域。 ### 5.3.2 SURF SIFT有着较好的效果，但是计算很耗时，这个特征计算也很复杂，大家就想怎么去改进它，因为它在找特征点时，一个卷积操作是比较耗时的，第二个就是在找到极值点后，还需要计算一个海森矩阵来去除边缘点，这一步也是比较耗时的，所以有学者就在想怎么去优化这几步。于是SURF诞生了。
SURF全称为Speeded Up Robust Features，由Herbert Bay等人在2006年提出。SURF算法主要分为三个步骤：尺度空间极值检测、关键点定位和方向分配。在尺度空间极值检测中，SURF使用了Hessian矩阵来检测图像中的稳定特征点。在关键点定位中，SURF使用了一种叫做“盒子滤波器”的算法来定位关键点。在方向分配中， SURF使用了一个类似于SIFT的方法来确定每个关键点的主方向。
与SIFT相比，SURF具有更快的计算速度和更好的尺度不变性。
**1.极值检测**
SURF检测极值点的方式是利用海森矩阵，该海森矩阵是原图像经过高斯核处理后的多尺度图像。在尺度σ下，点x=(x,y)处的Hessian值为：
$H(\boldsymbol{x}, \sigma)=\left[\begin{array}{ll} L_{x x}(\boldsymbol{x}, \sigma) & L_{x y}(\boldsymbol{x}, \sigma) \\ L_{y x}(\boldsymbol{x}, \sigma) & L_{y y}(\boldsymbol{x}, \sigma) \end{array}\right]$
其中Lxx(x,σ)是二维高斯核与图像做卷积后的二阶微分，但这个卷积很耗时，SURF的思想就是，用常值代替微分值，然后使用积分图加速卷积过程。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683038804204-5574ba92-fce2-41fd-b282-fae11e88638b.png#averageHue=%23e3e2e2&clientId=u8bf38a8a-db5a-4&from=paste&height=222&id=u04f27ce3&originHeight=277&originWidth=829&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=36478&status=done&style=none&taskId=u5a20d3ea-ff60-40c1-8570-726bb3ce208&title=&width=663.2)
从左到右：在y方向和xy方向上的（离散和裁剪的）高斯二阶偏导数，以及我们使用盒滤波器对其的近似。灰色区域等于零。
右边尺度的核，是左边高斯核的近似，称为盒式滤波器。而配合盒式滤波器使用的，是积分图。
积分图主要的思想是将图像从起点开始到各个点所形成的矩形区域像素之和作为一个数组的元素保存在内存中，当要计算某个区域的像素和时可以直接索引数组的元素，不用重新计算这个区域的像素和，从而加快了计算。
求取积分图时，对图像所有像素遍历一遍，得到积分图后，计算任何矩形区域内的像素灰度和只需进行三次加减运算，如下图所示。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683038946346-c39b8353-bc01-4482-8dd7-5aa878e1325d.png#averageHue=%23eeebdf&clientId=u8bf38a8a-db5a-4&from=paste&id=ub8e3d2eb&originHeight=286&originWidth=1816&originalType=url&ratio=1.25&rotation=0&showTitle=false&size=42030&status=done&style=none&taskId=u49a88064-a6d9-447b-b6a1-70f79d380b3&title=)
图中绿色区域的面积为：A-B-C+D，因为在减法的过程中，多减了一次D，所以最后要加上一个D。
因为不是用的真正的高斯核，所以最后海森矩阵的值不太准确，为了和真实高斯核值接近，这里使用了如下方式计算海森矩阵行列式
每个像素的Hessian矩阵行列式的近似值：
$\operatorname{det}(H)=D_{x x} * D_{y y}-w * D_{x y}^{2}$
滤波器响应的相关权重$w$是为了平衡Hessian行列式的表示式。这是为了保持高斯核与近似高斯核的一致性。
$w=\frac{\left|L_{x y}(\sigma)\right|_F\left|D_{x x}(\sigma)\right|_F}{\left|L_{x x}(\sigma)\right|_F\left|D_{x y}(\sigma)\right|_F}=0.912$
其中$|X|_F$为Frobenius范数。理论上来说对于不同的$\sigma$的值和对应尺寸的模板尺寸，$w$值是不同的，但为了简化起见，可以认为它是同一个常数0.9。在实际计算滤波响应值时，需要使用模板中盒式区域的面积进行归一化处理，以保证一个统一的Frobenius范数能适应所有的滤波尺寸。
这样就得到了每个像素的“斑点（blob)”响应值，且这个值是过滤掉了边缘点的。不需要在定位后再去除一遍。
接下来需要构建尺度空间了，与sift的既有降采样，又有高斯模糊不同，Surf没有降采样。在Surf中，不同组间图像的尺寸都是一致的，但不同组间使用的盒式滤波器的模板尺寸逐渐增大(核长宽越来越大)，同一组间不同层间使用相同尺寸的滤波器，但是滤波器的模糊系数逐渐增大（模糊系数是啥）。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683039596515-41523485-73b4-44fd-b59a-24fbb4765717.png#averageHue=%23e9e9e9&clientId=u8bf38a8a-db5a-4&from=paste&height=173&id=u47335664&originHeight=216&originWidth=531&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=53210&status=done&style=none&taskId=ud7218f27-67cb-44af-9129-fe40d02f8cc&title=&width=424.8)
在sift中，通过不断地减小图像尺寸来模拟远近变化，而SURF是通过改变fiter的尺寸来实现不同尺度的获得，不会因为filter尺寸的变大而使得计算量变大，因为在卷积是仅仅是查找积分图中的四个角处的数值，所有层的计算代价都是相同的。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683039633415-316779de-fa36-442f-902e-8f4c48709c80.png#averageHue=%23fafafa&clientId=u8bf38a8a-db5a-4&from=paste&height=257&id=u60791e90&originHeight=321&originWidth=676&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=73243&status=done&style=none&taskId=u358cc245-273b-480a-a02c-144121a5f1e&title=&width=540.8)
SURF的尺度空间中不同层所用的filter size如上图所示。
特征点的定位过程Surf和Sift保持一致，将经过Hessian矩阵处理的每个像素点与二维图像空间和尺度空间邻域内的26个点进行比较，初步定位出关键点，再经过滤除能量比较弱的关键点以及错误定位的关键点，筛选出最终的稳定的特征点。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683039780584-352d6b95-d136-45db-9c00-43be1d8e6a4c.png#averageHue=%23ededed&clientId=u8bf38a8a-db5a-4&from=paste&height=447&id=u7b0331da&originHeight=559&originWidth=655&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=197443&status=done&style=none&taskId=ud38aabaf-1203-4af4-ba1b-bea5bd6430d&title=&width=524)
同sift相同，取得极值位置后，还需要对极值进行二次精确定位，具体方法和sift中相同。 **2.方向计算**
Sift特征点方向分配是采用在特征点邻域内统计其梯度直方图，而在Surf中，采用的是统计特征点圆形邻域内的haar小波特征。小波特征也可以看做是和盒式滤波器相同的核。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683039974180-0fa7f420-dde1-4a18-9413-05346d8bf42b.png#averageHue=%23afafaf&clientId=u8bf38a8a-db5a-4&from=paste&height=156&id=u39fdf3dc&originHeight=195&originWidth=386&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=5386&status=done&style=none&taskId=u7b81a78a-77f0-4735-b5f4-762c3431140&title=&width=308.8)
在特征点的圆形邻域内，统计60度扇形内所有点的水平、垂直haar小波特征总和，然后扇形以一定间隔进行旋转并再次统计该区域内haar小波特征值之后，最后将值最大的那个扇形的方向作为该特征点的主方向。
**统计60度扇形内所有点的水平、垂直haar小波特征总和**
其具体方法是首先在特征点位置画一个直径为6s的圆形，s为尺度。分别计算这个圆中的每个以s*s为间隔取样的点处的haarX和haarY特征，同时计算每个点的特征方向 ：
$\theta _{i} = arctan(\frac{harrY}{haarX})$
在一个60度的扇形中统计落在扇形角度范围内的点的harrX和HarrY各自之和sumX和sumY，该扇形以每次15度的精度绕中心旋转，选取使得sum的模长最大的扇形方向为该特征点的方向，其中，harr小波特征的计算需要再次用到积分图。
使用$\sigma=2s$的高斯函数对Haar小波的响应值进行加权。为了求主方向，设计一个以特征点为中心，张角为$\pi/3$的扇形窗口，以一定旋转角度$\theta$旋转窗口，并对窗口内的Haar小波响应值$dx$ , $dy$进行累加，得到一个矢量(mw, θw) :
$m_w = \sum_{w} dx+\sum_{w} dy$
$\theta=arctan(\sum_{w} dy/\sum_{w} dx)$
主方向为最大Haar响应累加值所对应的方向，即$\theta=\theta_w|max \{ m_w\}$，当存在大于主峰值80%以上的峰值时，则将对应方向认为是该特征点的辅方向。一个特征点可能会被指定多个方向，可以增强匹配的鲁棒性。其过程示意图如下:
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683040500602-c76c4e24-75ab-42d3-804d-3d0e686404d2.png#averageHue=%23f9f8f8&clientId=u8bf38a8a-db5a-4&from=paste&height=294&id=u97d17f53&originHeight=368&originWidth=1066&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=171305&status=done&style=none&taskId=u78774ce9-1bd8-44cd-805a-39b4672087f&title=&width=852.8)
如图所示，求取主方向时扇形滑动窗口围绕特征点转动，统计Haar小波响应值，并计算方向角。
**3.描述子计算**
在Sift中，是取特征点周围4*4个区域块，统计每小块内8个梯度方向，用着4*4*8=128维向量作为Sift特征的描述子。
Surf算法中，也是在特征点周围取一个4*4的矩形区域块，但是所取得矩形区域方向是沿着特征点的主方向。每个子区域统计25个像素的水平方向和垂直方向的haar小波特征，这里的水平和垂直方向都是相对主方向而言的。该haar小波特征为水平方向值之后、垂直方向值之后、水平方向绝对值之后以及垂直方向绝对值之和4个方向。该过程示意图如下：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683040555937-31b5a595-8984-4dcb-9c80-743c63c867c2.png#averageHue=%23e4e3b8&clientId=u8bf38a8a-db5a-4&from=paste&height=378&id=u8185fad3&originHeight=472&originWidth=890&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=247661&status=done&style=none&taskId=u8ca0a9c6-f70e-476c-8906-2181d206c9a&title=&width=712)
把这4个值作为每个子块区域的特征向量，所以一共有4*4*4=64维向量作为Surf特征的描述子，比Sift特征的描述子减少了2倍。
SURF描述子不仅具有尺度和旋转不变性，还具有光照不变性，这由小波响应本身决定，而对比度不变性则是通过将特征向量归一化来实现。从下图为3种简单模式图像及其对应的特征描述子可以看出，引入Haar小波响应绝对值的统计和是必要的，否则只计算 :
$\sum dx,\sum dy$的话，第一幅图和第二幅图的特征表现形式是一样的，因此，采用4个统计量描述子区域使特征更具有区分度。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683040725886-ef3613a4-c351-4547-9c8d-4ea3259cbfa3.png#averageHue=%23c3c2c2&clientId=u8bf38a8a-db5a-4&from=paste&height=178&id=u7169f765&originHeight=223&originWidth=541&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=27109&status=done&style=none&taskId=u08dfc127-b933-4dd4-9483-8b8562a6f7b&title=&width=432.8)
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683040752878-556ff7d6-0cce-4220-a003-35e36ee50e82.png#averageHue=%23eeeae6&clientId=u8bf38a8a-db5a-4&from=paste&height=194&id=u35b9f477&originHeight=243&originWidth=784&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=46122&status=done&style=none&taskId=u085fe3b4-f5b0-49db-a724-f31dc1b4a0d&title=&width=627.2)
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683040767170-a3a9f99a-ab71-49a7-8452-16e1c4ee647d.png#averageHue=%23e2dfdf&clientId=u8bf38a8a-db5a-4&from=paste&height=251&id=ue7ee98b7&originHeight=314&originWidth=511&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=54554&status=done&style=none&taskId=ue1197398-e276-42a9-80c9-d538b6884e4&title=&width=408.8)
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683040779500-de587c8e-5165-4795-9485-f1ae41130dda.png#averageHue=%23f2f0ee&clientId=u8bf38a8a-db5a-4&from=paste&height=122&id=ub6553b91&originHeight=152&originWidth=809&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=25708&status=done&style=none&taskId=ua7a16add-d856-4b9c-a8ce-f037f6b209c&title=&width=647.2) ### 5.3.3 KAZE KAZE 算法（KAZE 是日语 "风" 的意思）是一种用于特征检测和描述的计算机视觉算法。与 SIFT 和 SURF 算法类似，KAZE 也构建了尺度空间（scale-space）来在不同尺度的图像中检测关键点。KAZE 与其他算法的主要区别在于它使用了非线性尺度空间（non-linear scale-space）。
KAZE 构建尺度空间的步骤如下： 1. **构建非线性尺度空间**：KAZE 使用一种叫做 "非线性扩散滤波"（non-linear diffusion filtering）的方法来构建尺度空间。对于每个尺度层，KAZE 使用一个不同的时间参数（t）来对原始图像进行非线性扩散滤波。这里的时间参数（t）相当于其他尺度空间中的高斯滤波器的方差（σ²）。 2. **计算特征响应**：KAZE 使用一个叫做 "Hessian 矩阵行列式"（Determinant of Hessian matrix）的方法来计算每个像素点的特征响应。特征响应越大，表示该点越可能是一个关键点。 3. **关键点检测**：KAZE 使用了一个类似于 SIFT 的方法来检测关键点。对于每个尺度层，KAZE 首先找到特征响应的局部极值点（在 3x3x3 的邻域内比邻居的响应更强），然后用一个阈值对特征响应进行筛选，最后保留特征响应较大的点作为关键点。 4. **关键点描述**：对于每个关键点，KAZE 提取一个特征描述子。描述子是一个向量，用于表示关键点周围的图像信息。KAZE 描述子是基于梯度直方图的，与 SIFT 描述子类似，但使用了不同的方法来计算梯度。KAZE 描述子具有较好的旋转不变性和尺度不变性。 具体步骤如下：
**1.使用非线性扩散滤波构建尺度空间**
(1) 非线性扩散滤波
非线性扩散滤波方法是将图像亮度（L）在不同尺度上的变化视为某种形式的流动函数的散度，可以通过非线性偏微分方程来描述：
$\frac{\partial L}{\partial t}=\operatorname{div}(c(x, y, t) \cdot \nabla L)$
其中c(x,y,t)为传导函数，可由下式来构造：
$c(x, y, t)=g\left(\left|\nabla L_{\sigma}(x, y, t)\right|\right)$
其中的▽Lσ是高斯平滑后的图像Lσ的梯度，《KAZE Features》一文中给出了g()函数的几种表达形式：
$\begin{aligned} & g_1=\exp \left(-\frac{\left|\nabla L_\sigma\right|^2}{k^2}\right), g_2=\frac{1}{1+\frac{\left|\nabla L_\sigma\right|^2}{k^2}} \\ & g_3=\left\{\begin{array}{cl} 1 & ,\left|\nabla L_\sigma\right|^2=0 \\ 1-\exp \left(-\frac{3.315}{\left(\left|\nabla L_\sigma\right| / k\right)^8}\right) & ,\left|\nabla L_\sigma\right|^2>0 \end{array}\right. \\ & \end{aligned}$
其中g1能够保留高对比度的边缘，g2能够保留宽度较大的区域，g3能够有效平滑区域内部而保留边界信息，《KAZE Features》一文中选择g2。参数k是控制扩散级别的对比度因子，决定保留多少边缘信息，其值越大，保留的边缘信息越少。
(2)AOS算法
非线性扩散滤波中的偏微分方程没有解析解。因此，需要使用数值方法来逼近微分方程。将上述偏微分方程离散化：
$\frac{L^{i+1}-L^i}{\tau}=\sum_{l=1}^m A_l\left(L^i\right) L^{i+1}$
其中Al是表示图像在各维度（l）上传导性的矩阵，τ为时间步长。该方程的解如下：
$L^{i+1}=\left(I-\tau \sum_{l=1}^m A_l\left(L^i\right)\right)^{-1} L^i$ 与SIFT不同的是，KAZE无需对图像进行下采样，各个层级均采用与原始图像相同的分辨率。非线性扩散滤波模型是以时间为单位的，故需要将尺度参数转换为时间（此处称为进化时间）。在高斯尺度空间下，使用标准差为σ的高斯核对图像进行卷积，相当于对图像进行持续时间为t=σσ/2的滤波，故两者的转换公式为：t=σσ/2。根据一组进化时间，利用AOS算法即可得到非线性尺度空间的所有图像：
$L^{i+1}=\left(I-\left(t_{i+1}-t_i\right) \sum_{i=1}^m A_i\left(L^i\right)\right)^{-1} L^i$
**2.多尺度空间检测极值点**
KAZE的特征点检测与SURF类似。是通过寻找不同尺度归一化后的Hessian行列式的局部极大值（或者极小值）点来实现的。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683080360382-88dcc274-9811-4ff5-be00-177d425315b6.png#averageHue=%23efefef&clientId=ub63f5f97-eb7f-4&from=paste&height=242&id=u70c37f33&originHeight=303&originWidth=356&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=85866&status=done&style=none&taskId=u56265f1a-a770-449f-9be8-1b1ad7d8515&title=&width=284.8)
如上图中画×的特征点，比较其与同一层中周围的8个像素点，再加上相邻层的9+9=18个像素点，即26个像素点，当其大于（或者小于）所有相邻点时，该点就是极值点。
**3.极值点精确定位**
这一过程和SIFT一样，通过泰勒表达式：
$L(\mathbf{x})=L+\left(\frac{\partial L}{\partial \mathbf{x}}\right)^T \mathbf{x}+\frac{1}{2} \mathbf{x}^T \frac{\partial^2 L}{\partial \mathbf{x}^2} \mathbf{x}$
对其求导数，并令导数等于零，解得
$\hat{\mathbf{x}}=-\left(\frac{\partial^2 L}{\partial \mathbf{x}^2}\right)^{-1} \frac{\partial L}{\partial \mathbf{x}}$
将解带入泰勒表达式，得：
$L(\hat{X})=L+\frac{1}{2}\left(\frac{\partial L}{\partial X}\right)^T \hat{X}$
$|L(\hat{X})| \geq T$
时保留该特征点，否则剔除。
**4.特征点主方向计算**
这一过程和SURF一样。若特征点的尺度参数为σi，则搜索半径设为6σi。在这个圆形领域内做一个60度的扇形区域，统计这个扇形区域内的haar小波特征总和，然后转动扇形区域，再统计小波特征总和。小波特征总和最大的方向为主方向。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683080567225-c6fb1bef-f705-4cdc-a7ea-3f2508d90c68.png#averageHue=%23f8f7f7&clientId=ub63f5f97-eb7f-4&from=paste&height=151&id=u1a9eeec6&originHeight=189&originWidth=564&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=62190&status=done&style=none&taskId=ue411e049-9268-46bb-86da-9a71e8b4b9f&title=&width=451.2)
**5.生成特征点描述子**
对于尺度参数为σi的特征点，在梯度图像上以特征点为中心取一个24σi×24σi的窗口，并将窗口划分为4×4个子区域，每个子区域大小为9σi×9σi，相邻的子区域有宽度为2σi的交叠带（此处我认为应该是相邻的子区域有宽度为4σi的交叠带，不然24σi不够划分为4×4个子区域）。每个子区域都用一个高斯核（σ1 =2.5σi）进行加权，然后计算出长度为4的子区域描述向量：
$d_v=\left(\sum L_x, \Sigma L_y, \Sigma\left|L_x\right|, \Sigma\left|L_y\right|\right)$
再通过另一个大小为4×4的高斯窗口（σ2 =1.5σi）对每个子区域的向量dv进行加权，最后进行归一化处理。这样就得到了4×4×4=64维的描述向量。
到此KAZE特征检测算法的原理算是讲完了，下面说一下KAZE特征的优缺点。
优点：
(1)在图像模糊、噪声干扰和压缩重构等造成的信息丢失的情况下，KAZE特征的鲁棒性明显优于其它特征。
(2)相比于线性尺度空间，非线性尺度空间不会造成边界模糊和细节丢失，而且更稳定。
缺点：
(1)KAZE算法在尺度不变性上是逊于SIFT的。
(2)KAZE特征的匹配对参数的设置比较敏感。
(3)KAZE特征的检测时间高于SURF，但与SIFT相近。 ### 5.3.4 ORB 简单来说，ORB = Oriented FAST（特征点） + Rotated BRIEF（特征描述）
FAST速度是快，但是无法体现出一个优良特征点的尺度不变性和旋转不变性。Fast角点本不具有方向，由于特征点匹配需要，ORB对Fast角点进行了改进，改进后的 FAST 被称为 Oriented FAST，具有旋转和尺度的描述。
**那么，Oriented FAST是怎么解决这个问题呢？**
从SIFT过来的我们对这个问题不陌生。 - 尺度不变性：可以用金字塔解决； - 旋转不变性：可以用质心标定方向解决； **尺度不变性：** 1. 对图像做不同尺度的高斯模糊 2. 对图像做降采样(隔点采样) 3. 对每层金字塔做FAST特征点检测 4. n幅不同比例的图像提取特征点总和作为这幅图像的oFAST特征点。 **旋转不变性：**
1、在一个小的图像块 B 中，定义图像块的矩。
$m_{p q}=\sum_{x, y} x^p y^q I(x, y)$
2、通过矩可以找到图像块的质心
$C=\left(\frac{m_{10}}{m_{00}}, \frac{m_{01}}{m_{00}}\right)$
3、连接图像块的几何中心 O 与质心 C，得到一个方向向量 OC→ ，这就是特征点的方向。
$\theta=\operatorname{atan} 2\left(m_{01}, m_{10}\right)$
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683210621520-035d7510-2b69-47dc-b055-0eaf7e803beb.png#averageHue=%23faf8f8&clientId=u84dae985-42ee-4&from=paste&height=306&id=woSnq&originHeight=382&originWidth=783&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=118455&status=done&style=none&taskId=u37474cda-5be0-4076-ac29-7da8f90f14b&title=&width=626.4)
**BRIEF**
BRIEF是2010年的一篇名为《BRIEF:Binary Robust Independent Elementary Features》的文章中提出，BRIEF是对已检测到的特征点进行描述，它是一种二进制编码的描述子，摒弃了利用区域灰度直方图描述特征点的传统方法，采用二进制、位异或运算，大大的加快了特征描述符建立的速度，同时也极大的降低了特征匹配的时间，是一种非常快速，很有潜力的算法。
如果说FAST用来解决寻找特征点的速度问题，那么BRIEF就用来解决描述子的空间占用冗余问题。
**特征点怎么来？**
好了，我们知道了BRIEF的实质就是特征点的描述子。那么前提就是描述对象特征点从何而来？实际上，Harris/FAST/SIFT/SURF等算法提供的特征点都可以。
**描述子怎么加？**
先定格调：在关键点周围做像素值比较，得到的结果是二进制串。
那下面就看一看它是如何给这些特征点加上描述子的：
1、为减少噪声干扰，先对图像进行高斯滤波（方差为2，高斯窗口为9x9）
2、以特征点为中心，取SxS的邻域窗口。在窗口内随机选取一对（两个）点，比较二者像素的大小，进行如下二进制赋值。
$\tau(\mathbf{p} ; \mathbf{x}, \mathbf{y}):= \begin{cases}1 & : \mathbf{p}(\mathbf{x})<\mathbf{p}(\mathbf{y}) \\ 0 & : \mathbf{p}(\mathbf{x}) \geq \mathbf{p}(\mathbf{y})\end{cases}$
其中，p(x)，p(y)分别是随机点x=(u1,v1),y=(u2,v2)的像素值。 3、在窗口中随机选取N对随机点，重复步骤2的二进制赋值，形成一个二进制编码，这个编码就是对特征点的描述，即特征描述子。（一般N=256）
这个特征可以由n位二进制测试向量表示，BRIEF描述子：
$f_n(\mathbf{p}):=\sum_{1 \leq i \leq n} 2^{i-1} \tau\left(\mathbf{p} ; \mathbf{x}_i, \mathbf{y}_i\right)$
这里面，最关键的地方其实是随机特征对的选取，论文中就给出了5种方法（其中第二种比较好)，分别为:
1.$x_i, y_i$都呈均匀分布$\left[-\frac{S}{2}, \frac{S}{2}\right]$；
2.$x_i, y_i$都呈高斯分布$\left[0, \frac{1}{25} S^2\right]$，准则采样服从各向同性的同一高斯分布;
3.$x_i$服从高斯分布$\left[0, \frac{1}{25} S^2\right]$，$y_i$ 服从高斯分布$\left[x_i, \frac{1}{100} S^2\right]$，采样分两步进行: 首先在原 点处为$x_i$进行高斯采样，然后在中心为$x_i$处为$y_i$ 进行高斯采样;
4.$x_i, y_i$在空间量化极坐标下的离散位置处进行随机采样;
5.$x_i=(0,0)^T$，$y_i$在空间量化极坐标下的离散位置处进行随机采样;
这5种方法生成的256对(OpenCVi中用32个字节存储这256对)随机点如下（一条线段的两个端点是一对）： ![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683210892351-ce5e22a2-8334-4e70-803a-ff4e0a65b934.png#averageHue=%23e2e2e2&clientId=u84dae985-42ee-4&from=paste&height=193&id=eYatQ&originHeight=241&originWidth=784&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=184974&status=done&style=none&taskId=uc1dab4bc-1b6e-40d5-847e-530c7cfdaa0&title=&width=627.2)
经过上面三个步骤，我们就可以为每个特征点表示为一个256bit的二进制编码。
Rotated BRIEF
在介绍ORB的改善之前，我们先思考一个问题。描述子是用来描述一个特征点的属性的，除了标记特征点之外，它最重要的一个功能就是要实现特征点匹配。BRIEF是如何实现特征点匹配的呢？
答案是：Hamming距离！
汉明距离是使用在数据传输差错控制编码里面的，汉明距离是一个概念，它表示两个（相同长度）字对应位不同的数量，我们以d（x,y）表示两个字x,y之间的汉明距离。对两个字符串进行异或运算，并统计结果为1的个数，那么这个数就是汉明距离。 1. 两个特征编码对应bit位上相同元素的个数小于128的，一定不是配对的。 2. 一幅图上特征点与另一幅图上特征编码对应bit位上相同元素的个数最多的特征点配成一对。 其实就是**按位求异或**的过程。（相同为0，不同为1）
所以，对于BRIEF来说，描述子里不包含旋转属性，所以一旦匹配图片有稍微大点的旋转角度，按照Hamming算法，匹配度将会大幅下降。 **ORB如何优化？**
**首先，做一些前期优化：** 1. ORB算法进一步增强描述子的抗噪能力，采用积分图像来进行平滑； 2. 在特征点的31x31邻域内，产生随机点对，并以随机点为中心，取5x5的子窗口。 3. 比较两个随机点的子窗口内25个像素的大小进行编码（而不仅仅是两个随机点了） 其次，为BRIEF增加旋转不变性（Steered BRIEF）： ORB算法采用关键点的主方向来旋转BEIEF描述子。 1、对于任意特征点，在31x31邻域内位置为(xi,yi)的n对点集，可以用2 x n的矩阵来表示：
$S=\left(\begin{array}{l} x_1, \cdots, x_n \\ y_1, \cdots, y_n \end{array}\right)$
2、利用FAST求出的特征点的主方向$\theta$和对应的旋转矩阵$R_\theta$，算出旋转的$S_\theta$来代表$S$:
$\begin{aligned} & \theta=\operatorname{atan} 2\left(m_{01}, m_{10}\right) \\ & R_\theta=\left[\begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right] \\ & S_\theta=R_\theta S \end{aligned}$
3、计算旋转描述子（**steered BRIEF**）:
$g_n(p, \theta):=f_n(p) \mid\left(x_i, y_i\right) \in S_\theta$
其中$f_n(\mathbf{p}):=\sum_{1 \leq i \leq n} 2^{i-1} \tau\left(\mathbf{p} ; \mathbf{x}_i, \mathbf{y}_i\right)$为BRIEF的描述子
**最后，rBRIEF-改进特征点描述子的相关性**
使用steeredBRIEF方法得到的特征描述子具有旋转不变性，但是却在另外一个性质上不如原始的BRIEF算法。是什么性质呢，是描述符的可区分性，或者说是相关性。这个性质对特征匹配的好坏影响非常大。描述子是特征点性质的描述。描述子表达了特征点不同于其他特征点的区别。我们计算的描述子要尽量的表达特征点的独特性。如果不同特征点的描述子的可区分性比较差，匹配时不容易找到对应的匹配点，引起误匹配。ORB论文中，作者用不同的方法对100k个特征点计算二进制描述符，对这些描述符进行统计，如下表所示：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683211140788-3f708cca-c807-4e43-a92c-6f665fcd956b.png#averageHue=%23fbf7f7&clientId=u84dae985-42ee-4&from=paste&height=472&id=YsIhb&originHeight=590&originWidth=795&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=105293&status=done&style=none&taskId=u52586628-4379-4546-9b67-9728adef55c&title=&width=636)
我们先不看rBRIEF的分布。对BRIEF和steeredBRIEF两种算法的比较可知，BRIEF算法落在0上的特征点数较多，因此BRIEF算法计算的描述符的均值在0.5左右，每个描述符的**方差较大，可区分性较强**。而steeredBRIEF失去了这个特性。
至于为什么均值在0.5左右，方差较大，可区分性较强的原因，这里大概分析一下。这里的描述子是二进制串，里面的数值不是0就是1，如果二进制串的均值在0.5左右的话，那么这个串有大约相同数目的0和1，那么方差就较大了。用统计的观点来分析二进制串的区分性，如果两个二进制串的均值都比0.5大很多，那么说明这两个二进制串中都有较多的1时，在这两个串的相同位置同时出现1的概率就会很高。那么这两个特征点的描述子就有很大的相似性。这就增大了描述符之间的相关性，减小之案件的可区分性。
下面我们介绍解决上面这个问题的方法：**rBRIEF**。
原始的BRIEF算法有5种去点对的方法，原文作者使用了方法2。为了解决描述子的可区分性和相关性的问题，ORB论文中没有使用5种方法中的任意一种，而是使用统计学习的方法来重新选择点对集合。 **首先，**建立300k个特征点测试集。
备注：
对于测试集中的每个点，预处理参考第一个步骤。考虑其31x31邻域。这里不同于原始BRIEF算法的地方是，这里在对图像进行高斯平滑之后，使用邻域中的某个点的5x5邻域灰度平均值来代替某个点对的值，进而比较点对的大小。这样特征值更加具备抗噪性。另外可以使用积分图像加快求取5x5邻域灰度平均值的速度。
**其次，**特征点选取
从上面可知，在31 x 31的邻域内共有(31-5+1)x(31-5+1)=729个这样的子窗口，那么取点对的方法共有M=205590种，我们就要在这Ｍ种方法中选取256种取法，选择的原则是这256种取法之间的相关性最小。怎么选取呢？ - 在300k特征点的每个31x31邻域内按M种方法取点对，比较点对大小，形成一个300k x M的二进制矩阵Q。矩阵的每一列代表300k个点按某种取法得到的二进制数。 - 对Q矩阵的每一列求取平均值，按照平均值到0.5的距离大小重新对Q矩阵的列向量排序，形成矩阵T。 - 将T的第一列向量放到R中 - 取T的下一列向量和R中的所有列向量计算相关性，如果相关系数小于设定的阈值，则将T中的该列向量移至R中。 - 按照第四步的方式不断进行操作，直到R中的向量数量为256。 - 通过这种方法就选取了这256种取点对的方法。这个算法是对均值靠近0.5的不相关测试进行贪婪搜索，结果称为rBRIEF。rBRIEF在方差和相关性上与旋转BRIEF相比有明显进步（如图4）。PCA的特征值较高，它们的下降速度要快得多。 ![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683211317473-34b18f93-24b5-4ade-94ca-e39d20407e38.png#averageHue=%23fbfafa&clientId=u84dae985-42ee-4&from=paste&height=362&id=u10gZ&originHeight=453&originWidth=782&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=114176&status=done&style=none&taskId=u29696271-a597-44c0-98ed-b6bcd7dafe4&title=&width=625.6)
至此，ORB的优化就结束了。我们尝试总结一下：
FAST是用来寻**找特征点**的。ORB在FAST基础上通过金字塔、质心标定解决了尺度不变和旋转不变。即oFAST。
BRIEF是用来**构造描述子**的。ORB在BRIEF基础上通过引入oFAST的旋转角度和机器学习解决了旋转特性和特征点难以区分的问题。即rBRIEF.
现在，有了特征点寻找和描述子，ORB就成了！ ## **5.4 常见的关键点检测算法** ### 5.4.1 Harris角点 Harris角点是这样的，假设在一个图像上画一个窗口，在平面中，窗口向任意一个方向移动，窗口内像素灰度不会有太大变化，如果是在一条线段上，平行于线段移动，窗口内像素灰度不会有太大变化，而垂直于线段移动则会有较大变化。如果是角点的话，朝任意方向移动，窗口内像素灰度都会有较大变化。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683206511234-8baa7991-11a7-4179-86c4-84bea2e60685.png#averageHue=%23d3d3d3&clientId=u84dae985-42ee-4&from=paste&height=466&id=u1e186656&originHeight=582&originWidth=1216&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=159905&status=done&style=none&taskId=u8f2f14bc-99d9-409b-8806-ae2d68491ed&title=&width=972.8)
使用数学表达式如下：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683206549138-abf06d96-f642-4d7a-99ee-32411f652268.png#averageHue=%23faf2f2&clientId=u84dae985-42ee-4&from=paste&height=300&id=uf1c1cfe6&originHeight=375&originWidth=1172&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=95413&status=done&style=none&taskId=u7b918ca6-d3aa-4ea3-bc97-a49f262b690&title=&width=937.6)
对E(u,v)表达式进行泰勒展开：
$I(x+u, y+v) \approx I(x, y)+u I_{x}+v I_{y}$
$\begin{aligned} E(u, v) & =\sum_{(x, y)} w(x, y) \times\left[I(x, y)+u I_{x}+v I_{y}-I(x, y)\right]^{2} \\ & =\sum_{(x, y)} w(x, y) \times\left(u I_{x}+v I_{y}\right)^{2} \\ & =\sum_{(x, y)} w(x, y) \times\left(u^{2} I_{x}^{2}+v^{2} I_{y}^{2}+2 u v I_{x} I_{y}\right) \end{aligned}$
其中：
其中 Ix和Iy 是 I的偏微分，在图像中就是在 x 和 y 方向的梯度图（可以通过cv2.Sobel()来得到）：
$I_x=\frac{\partial I(x, y)}{\partial x}, \quad I_y=\frac{\partial I(x, y)}{\partial y}$
把 u 和 v 拿出来，得到最终的形式：
$E(u, v) \approx[u, v] M\left[\begin{array}{l} u \\ v \end{array}\right]$
其中矩阵M为：
$M=\sum_{(x, y)} w(x, y)\left[\begin{array}{cc} I_x^2 & I_x I_y \\ I_x I_y & I_y^2 \end{array}\right] \rightarrow R^{-1}\left[\begin{array}{cc} \lambda_1 & 0 \\ 0 & \lambda_2 \end{array}\right] R$
最后是把实对称矩阵对角化处理后的结果，可以把R看成旋转因子，其不影响两个正交方向的变化分量。经对角化处理后，将两个正交方向的变化分量提取出来，就是 λ1 和 λ2（特征值）。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683206914996-20392abf-5137-4e63-a30d-30abe7d9f2c0.png#averageHue=%23f0edec&clientId=u84dae985-42ee-4&from=paste&height=512&id=u5c31cde0&originHeight=640&originWidth=852&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=213982&status=done&style=none&taskId=uce42d619-4b40-4c52-b580-6358454d1a3&title=&width=681.6)
**平坦区域**：两个特征值都小，且近似相等，能量函数在各个方向上都较小；
**边缘区域**：一个特征值大，另一个特征值小，能量函数在某一方向上增大，其他方向较小；
**角点区域**：两个特征值都大，且近似相等，能量函数在所有方向上都增大。 为了不用单独求特征值，检测出特征向量很大的角点，使用如下角点响应函数来取值。计算每个窗口对应的得分（角点响应函数R）：
$R=\operatorname{det}(M)-k(\operatorname{trace}(\mathrm{M}))^2$
k 是一个经验常数，在范围 (0.04, 0.06) 之间。
但还有最后一步，根据 R 的值，将这个窗口所在的区域划分为平面、边缘或角点。为了得到最优的角点，我们还可以使用**非极大值抑制**。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683207578246-fde8d07c-f7ba-420c-b1d9-d5d0d75df5c9.png#averageHue=%23e09675&clientId=u84dae985-42ee-4&from=paste&height=389&id=u829ce8e4&originHeight=486&originWidth=814&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=188825&status=done&style=none&taskId=u36879cf9-3fd1-4d88-8f80-dc1db61f137&title=&width=651.2)
注意：Harris 检测器具有旋转不变性，但不具有尺度不变性，也就是说尺度变化可能会导致角点变为边缘，如下图所示：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683207153232-4f3d5850-7bba-4d1d-a4c0-5a23302a0495.png#averageHue=%23fdfdfd&clientId=u84dae985-42ee-4&from=paste&height=176&id=u147c57f6&originHeight=220&originWidth=534&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=35465&status=done&style=none&taskId=u1d56e11a-4ff8-4c1a-bddb-d617611258c&title=&width=427.2)
### 5.4.2 Shi-Tomasi角点 Shi-Tomasi角点，就是常说得GFTT（Good Features to Track）角点，它是 Harris 角点检测的改进版，Shi-Tomasi 方法在很多情况下可以得到比 Harris 算法更好的结果。
Harris 角点检测中每个窗口的分数公式是将矩阵 M 的行列式与 M 的迹相减：
$R=\lambda_1 \lambda_2-k\left(\lambda_1+\lambda_2\right)^2$
由于 Harris 角点检测算法的稳定性和 k 值有关，而 k 是个经验值，不好设定最佳值。Shi-Tomasi 发现，角点的稳定性其实和矩阵 M 的较小特征值有关，于是直接用较小的那个特征值作为分数。这样就不用调整k值了。所以 Shi-Tomasi 将分数公式改为如下形式：
$R=\min \left(\lambda_1, \lambda_2\right)$
和 Harris 一样，如果该分数大于设定的阈值，我们就认为它是一个角点。
我们可以把它绘制到 λ1 ～ λ2 空间中，就会得到下图：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683207853108-5ef5a8f1-29fa-40f3-9400-88919c6fed92.png#averageHue=%23b4e483&clientId=u84dae985-42ee-4&from=paste&height=286&id=u5ef3d579&originHeight=357&originWidth=528&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=52055&status=done&style=none&taskId=u7a023c6a-96d8-48bb-8840-72992b677a8&title=&width=422.4) ### 5.4.3 Fast 上述Harris角点与Shi-Tomasi角点都是基于梯度计算的角点检测方法，其计算复杂度高，图像中的噪声可以阻碍梯度计算。想要提高检测速度的话，可以考虑基于模板的方法：FAST角点检测算法。该算法原理比较简单，但实时性很强。
（Features from Accelerated Segment Test）由Edward Rosten和Tom Drummond在2006年首先提出，是近年来倍受关注的基于模板和机器学习的角点检测方法，它不仅计算速度快，还具有较高的精确度。Fast算法的基本原理就是使用周长为16个像素点（半径为3的Bresenham圆）来判定其圆心像素是否为角点。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683209864888-7156b994-494e-473a-a0d1-6a0b9e93e52d.png#averageHue=%23a4a4a4&clientId=u84dae985-42ee-4&from=paste&height=304&id=u5b3d466a&originHeight=380&originWidth=750&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=132628&status=done&style=none&taskId=ue48e0d7d-5759-4880-a338-dbdcef7b068&title=&width=600)
在圆周上按顺时针方向从1到16的顺序对圆周像素点进行编号。设圆心像素p的亮度值为Ip，阈值为t。如果在圆周上有连续N个像素的亮度值比Ip+t大，或者比Ip-t小，则圆心像素被称为角点。
$\left\{\begin{array}{lll} \text { 条件 } 1: \text { 连续 } N \text { 个像素 } x \text { 的像素亮度值 } I_x: & I_x>\left(I_p+t\right), & \text { if } I_x>I_p \\ \text { 条件 } 2: \text { 连续 } N \text { 个像素 } x \text { 的像素亮度值 } I_x: & I_x<\left(I_p-t\right), & \text { if } I_x![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683210143552-8bfecd82-3b6d-4f1f-9917-75de1e96ba88.png#averageHue=%23e9e9e9&clientId=u84dae985-42ee-4&from=paste&height=62&id=u2b5530c0&originHeight=78&originWidth=732&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=16684&status=done&style=none&taskId=u6dcc3fe9-e354-496a-b701-dab9b88fd1a&title=&width=585.6)
Fast算法流程
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683210169560-cc7390b6-a7e4-4199-bb99-1b9b82830c0a.png#averageHue=%23ededed&clientId=u84dae985-42ee-4&from=paste&height=247&id=u7eb0ffba&originHeight=309&originWidth=800&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=55515&status=done&style=none&taskId=ub7a14b22-34ab-4721-a291-e221bc90d35&title=&width=640)
FAST非极大值抑制
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683210223077-b41ccd86-05af-4c0d-8249-c3ba13030237.png#averageHue=%23efefef&clientId=u84dae985-42ee-4&from=paste&height=263&id=u2d8118cd&originHeight=329&originWidth=800&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=47486&status=done&style=none&taskId=u64e0c6c1-9376-4763-b44b-c087b684a53&title=&width=640) ## 本章小结 本章介绍了常见视觉特征点，有运算复杂度比较高的SIFT，SURF，也有基于模板，实时性好的Shi-Tomasi，ORB特征点。特征点的追踪是前端VO的基础，也是整个前端最耗时的部分之一。 ## 本章思考 1.请说说SIFT或SURF的原理，并对比它们与ORB之间的优劣。
2.我们发现，OpenCV提供的ORB特征点在图像中分布不够均匀。你是否能够找到或提出让特征点分布更均匀的方法？ ## 附录 本章提到的大多数特征点，在opencv中都能找到对应接口，感兴趣的可以去看opencv的modules->features2d中对应实现，另外，在github上也有很多开源实现，在此列举几个：
sift的C++实现：[https://github.com/phoenix16/SIFT](https://github.com/phoenix16/SIFT)
surf的C++实现：[https://github.com/JairFrancesco/OpenSurf](https://github.com/JairFrancesco/OpenSurf)
KAZE：[https://github.com/pablofdezalc/kaze](https://github.com/pablofdezalc/kaze)
如果你想动手实践一下，可以用这个资料：[https://github.com/whoisraibolt/Feature-Detection-and-Description](https://github.com/whoisraibolt/Feature-Detection-and-Description) ## 参考 [1.SIFT特征点提取](https://blog.csdn.net/lingyunxianhe/article/details/79063547)
[2.SIFT算法](https://zhuanlan.zhihu.com/p/343522892)
[3.Surf算法特征点检测与匹配](https://blog.csdn.net/dcrmg/article/details/52601010)
[4.SURF特征提取算法](https://zhuanlan.zhihu.com/p/311415924)
[5.[基础知识] Speeded Up Robust Features （SURF特征）](https://zhuanlan.zhihu.com/p/365403867)
[6.图像处理基础（六）基于 LOG 的 blob 兴趣点检测](https://zhuanlan.zhihu.com/p/448959603)
[7.图像处理基础 （四）边缘提取之 LOG 和 DOG 算子](https://zhuanlan.zhihu.com/p/446286009)
[8.角点检测：Harris 与 Shi-Tomasi](https://zhuanlan.zhihu.com/p/83064609)
[9.【理解】经典角点检测算法--Harris角点](https://blog.csdn.net/SESESssss/article/details/106774854)
[10.FAST角点学习笔记](https://zhuanlan.zhihu.com/p/76501359)
[11.FAST角点检测学习笔记](https://zhuanlan.zhihu.com/p/489625338)
[12.（四十二）特征点检测-ORB](https://zhuanlan.zhihu.com/p/91479558)
[13.ORB特征提取详解](https://blog.csdn.net/zouzoupaopao229/article/details/52625678)
[14.图像特征算法(三)——ORB算法简述及Python中ORB特征匹配实践](https://zhuanlan.zhihu.com/p/261966288)
[15.ORB特征点检测](https://www.cnblogs.com/ronny/p/4083537.html) ================================================ FILE: docs/chapter6/chapter6.md ================================================ # **6.换一个视角看世界：前端-视觉里程计之对极几何** 荣誉和财富，若没有聪明才智，是很不牢靠的财产。——德谟克里特 --- > **本章主要内容** \ > 1.对极几何 \ > 2.本质矩阵及其求解 \ > 3.单应矩阵及其求解 \ > 4.三角测量 **对极几何**是**立体视觉中的几何关系**，描述相机从不同位置拍摄3D场景时，3D点与相机位姿，图像观测像素坐标之间的几何关系。这种几何关系可以作为约束应用到求解相机运动及特征点3D坐标中。
**关于立体视觉：** > 立体视觉(Stereo Vision)是什么呢？我们可以这样理解： > 立体视觉(StereoVision) = 寻找相关性(Correspondences) + 重建(Reconstruction) > - Correspondences:给定张图片中的像素P点，寻找其在另一张图片中的对应点Pr。 > - Reconstruction:给定一组对应点对(P,P,),计算其在空间中对应点P的3D坐标。 > ![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685265589313-38e0dd6a-8935-4178-8e6b-6517be012151.png#averageHue=%23e0dedc&clientId=ud185277d-3a15-4&from=paste&height=261&id=ucd4e513b&originHeight=326&originWidth=657&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=130765&status=done&style=none&taskId=u805942d2-4983-4a74-af33-f331d551ac8&title=&width=525.6) 对极几何中存在以下几个概念：对极点，对极线，对极平面。其中**对极线构成的约束称为对极线约束**，也称为**对极约束**。
![对极几何1.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685257312236-c843ef08-d537-49d7-9216-0660c1c2df8e.png#averageHue=%23d8f2bd&clientId=ud185277d-3a15-4&from=paste&height=245&id=uad61ea8f&originHeight=306&originWidth=457&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=39482&status=done&style=none&taskId=u9ba5f4d3-2984-4470-8e3b-905965c4f6d&title=&width=365.6)
**对极点：**左相机光心OL在右相机平面上的成像点eR，称为其中一个对极点，类似的，OR在左相机上的成像点eL，也是对极点。对极点是虚拟的点，如果相机之间不能观测到对方光心，则对极点会在图像之外。
**对极线：**在相机OL观测到一个点XL，实际情况该XL可能对应3D坐标中任意一个Xi，因为线**O**L**X**被因为与左相机中心重合而被左相机视为一个点。但对于右相机，每个Xi则将有不同观测，这些观测为其图像平面中的一条线，该线称为对极线。如图，右摄像机中的那条线（**e**R**X**R）就称为**对极线**。对称地，右相机视线**O**R**X**为一个点，而被左相机视为对极线（**e**L**X**L）。
对极线是3D空间中点**X**的位置的函数，一个兴趣点对应一组对极线（XLeL,XReR）。由于线**O**L**X**通过透镜**O**L的光学中心，因此右图中相应的对极线必须通过**e**R（并且对应于左图中的极线）。一幅图像中的所有对极线都包含该图像的对极点。
**对极平面：**趣点**X**与两相机中心**O**L、**O**R三点形成的平面称为**对极平面**。对极平面与每个相机的图像平面相交形成线即为对极线。无论**X**位于何处，所有对极平面和对极线都与对极点相交。
**基线：**两个相机光心相连的直线OLOR称为基线。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685258773597-f9f4ea5a-b764-4877-a000-ec9d3514c66f.png#averageHue=%236c725b&clientId=ud185277d-3a15-4&from=paste&height=266&id=MRkjP&originHeight=333&originWidth=842&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=396606&status=done&style=none&taskId=u5d8db893-fbe5-452d-bc8e-b71ec5f4e8b&title=&width=673.6) ## 6.1 对极约束 P点在图像_I1_中观测的位置是_P_1，在_I_2中观测的位置是_P_2，_O1_与_O2_为相机的光心。点P与_O1_，_O2_形成的平面称为极平面。极平面与图像平面的交线称为极线，即图中的l1与l2。其中e1与e2称为极点。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683984848664-6a891899-a7c6-4798-aa71-415b09695265.png#averageHue=%23f8f8f8&height=232&id=dOZVQ&originHeight=301&originWidth=603&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=&width=464)
假设_O_1相机坐标系下P点坐标为P(X,Y,Z)，归一化坐标为Pu（X1,Y1,1），则根据针孔相机投影模型(第一节内容，也可以参考本节附录第一节)，观测的像素坐标_P_1（_u__1__,v__1_）为：
$\tag{6.1} \left[\begin{matrix}u_1\\v_1\\1\\\end{matrix}\right]\ =\ \left[\begin{matrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\\\end{matrix}\right]\left[\begin{matrix}X_1\\Y_1\\1\\\end{matrix}\right]$
化为简洁的形式如下，其中K为相机内参：
$\begin{equation} \tag{6.2} \begin{array}{c} p_1=KP_u^1 \end{array} \end{equation}$
好了，现在假设_O__1_相机相对于_O2_的运动及旋转为t与R，那么根据坐标系变换的关系，P在_O2_坐标系下坐标为：
$P_2=RP_1+t$
同样，根据相机投影模型，可以得到观测像素坐标与局部三维坐标的关系为：
$p_2=KP_u^2=K\left(RP_1+t\right)_u$
为了描述对极约束，这里需要用到投影关系，即一个坐标等比例缩放的关系，物理含义是指它们是在同一条射线上，通过投影关系（对该处推导不理解的可以看附录中第二小节推导），可以得到在相机_O1_与_O2_下，对P点观测的归一化坐标关系为：
$p_u^2\ast\frac{1}{s_2}=Rp_u^1\ast\frac{1}{s_1}+t$
在等式左右同时左乘t^，上三角符号含义为取向量的反对称矩阵，运算结果为向量的外积：
$t^\land\ast p_u^2\ast\frac{1}{s_2}=t^\land Rp_u^1\ast\frac{1}{s_1}+t^\land t$
因为相同向量，外积为0，所以上式变为：
$t^\land\ast p_u^2\ast\frac{1}{s_2}=t^\land Rp_u^1\ast\frac{1}{s_1}$
两边同时乘以_p__2_的转置：
$\left(p_u^2\right)^T\ast t^\land\ast p_u^2\ast\frac{1}{s_2}=\left(p_u^2\right)^{T{\ast\ t}^\land}Rp_u^1\ast\frac{1}{s_1}$
其中左等式，t^p2u为一个与t及p2u垂直的向量（所以对极几何t一定不能为0，不然在推导这里就不成立），既然与自身垂直，那么两个垂直向量做内积，结果为0，左侧严格等于0。则此时去掉常数项也不会影响等式成立：
$\left(p_u^2\right)^Tt^\land Rp_u^1=0$
其中_p__1__u__，p__2__u_为物体在相机坐标系下的归一化坐标，其与物体真实坐标及像素的齐次坐标关系为：
$P_u^1=K^{-1}u_1^齐=s_1P^1$
通常有如下表示：
$E=t^\land R$
称E为对极几何中的本质矩阵（Essential Matrix）,如果把物体的归一化坐标换为像素齐次坐标，则有如下结果：
$u_2^齐K^{-T}t∧RK^{-1}u_1^齐=0$
其中有如下表示：
$F=K^{-T}EK^{-1}$
F包含内参，称为对极几何中的基础矩阵（Fundamental Matrix） ## 6.2 本质矩阵的求解-八点法 由上可知，一对匹配点，与本质矩阵的关系可以得到一个等式：
$\left(p_u^2\right)^TEp_u^1=0$
其中E矩阵为3x3矩阵，有9个未知数，但实际上E只有5个自由度，表明其最少可以用五个点来列方程来求解，但这五个自由度是建立在非线性性质之上的，求解比较复杂。如果只考虑其尺度等价性，则E有8个自由度，这种线性性质会让求解更简单些，所以就有了常用的8点法。设E为：
$E\ =\ \left[\begin{matrix}e_1&e_2&e_3\\e_4&e_5&e_6\\e_7&e_8&e_9\\\end{matrix}\right]$
则对极约束可以写为如下形式：
$\left[\begin{matrix}x_2&y_2&1\\\end{matrix}\right]\left[\begin{matrix}e_1&e_2&e_3\\e_4&e_5&e_6\\e_7&e_8&e_9\\\end{matrix}\right]\left[\begin{matrix}x_1\\y_1\\1\\\end{matrix}\right]\ =\ 0$
把E写为向量形式：
$e=\left[\begin{matrix}e_1&e_2&e_3\\\end{matrix}\ \ \ \begin{matrix}e_4&e_5&e_6\\\end{matrix}\ \ \ \begin{matrix}e_7&e_8&e_9\\\end{matrix}\right]^T$
则上式方程为：
$\left[\begin{matrix}x_2x_1&x_2y_1&x_2\\\end{matrix}\ \ \ \begin{matrix}y_2x_1&y_2y_1&y_2\\\end{matrix}\ \ \ \begin{matrix}x_1&y_1&1\\\end{matrix}\right]\ast e\ =\ 0$
使用8对匹配点，每一对匹配点构成上述的方程，那么就有8组方程，最后8组方程构成一个线性齐次方程组，这种将本质矩阵看做向量，然后通过求解线性方程组来获得矩阵的方式，也称为直接线性变换法（DLT）。如下：
$\left(\begin{array}{ccccccccc} x_{2}^{1} x_{1}^{1} & x_{2}^{1} y_{1}^{1} & x_{2}^{1} & y_{2}^{1} x_{1}^{1} & y_{2}^{1} y_{1}^{1} & y_{2}^{1} & x_{1}^{1} & y_{1}^{1} & 1 \\ x_{2}^{2} x_{1}^{2} & x_{2}^{2} y_{1}^{2} & x_{2}^{2} & y_{2}^{2} x_{1}^{2} & y_{2}^{2} y_{1}^{2} & y_{2}^{2} & x_{1}^{2} & y_{1}^{2} & 1 \\ & ... \\\ x_{2}^{8} x_{1}^{8} & x_{2}^{8} y_{1}^{8} & x_{2}^{8} & y_{2}^{8} x_{1}^{8} & y_{2}^{8} y_{1}^{8} & y_{2}^{8} & x_{1}^{8} & y_{1}^{8} & 1 \end{array}\right) e=0$
根据线性方程解的情况，左侧系数矩阵为8x9的矩阵，e一定存在非零解。求解该方程，就可以得到本质矩阵E的每个元素了。 ## 6.3 从本质矩阵恢复相机运动 在得到本质矩阵E之后，还需要从E中恢复相机的运动R与t。此时需要用到奇异值分解（SVD）,假设E的SVD为：
$E=U\sum V^T$
其中U与V为正交阵，∑为奇异值矩阵。有如下性质：∑ = diag(σ,σ,0)。于是可以配凑出t与R
$\begin{array}{c} t_{1}^{\wedge}=U R_{z}\left(\frac{\pi}{2}\right) \sum V^{\mathrm{T}}, R_{1}=U R_{z}^{T}\left(\frac{\pi}{2}\right) V^{T} \\ t_{2}^{\wedge}=U R_{z}\left(-\frac{\pi}{2}\right) \sum V^{\mathrm{T}}, R_{2}=U R_{z}^{T}\left(-\frac{\pi}{2}\right) V^{T} \end{array}$
其中：
$R_z\left(\frac{\pi}{2}\right)=\ \left[\begin{matrix}0&-1&0\\1&0&0\\0&0&1\\\end{matrix}\right],R_z\left(-\frac{\pi}{2}\right)=\ \left[-\begin{matrix}0&1&0\\1&0&0\\0&0&1\\\end{matrix}\right]$
由于E与-E的等价，这里t取负号也是成立的，所以一共有四组解：
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683984848931-e37a1257-3050-4379-bcbe-d7a6060c5e8c.png#averageHue=%23fdfdfd&id=f6kTT&originHeight=318&originWidth=966&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=)
其中只有第一种情况，P点在两个相机下具有正的深度，所以只要把任意一点求解出深度，在两个相机坐标系下深度都为正，就可以得到真实解了。 ## 6.4 单应矩阵 多视图几何中，除了本质矩阵和基础矩阵，还存在另一种常见的矩阵：单应矩阵(Homography)H，它描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（比如墙、地面等），则可以通过单应性进行运动估计。这种情况在无人机携带的俯视相机或扫地机携带的顶视相机中比较常见。
单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685264330326-0fc3f5e9-58dc-4936-aa11-f6aa196213a6.png#averageHue=%23f7f7f4&clientId=ud185277d-3a15-4&from=paste&height=250&id=u642c8eee&originHeight=434&originWidth=574&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=162489&status=done&style=none&taskId=u80520f70-d217-4a5e-9df6-d2f8f1544c6&title=&width=331.20001220703125)
单应性在SLAM中具有重要意义。当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，这就出现了所谓的退化(degenerate)。现实中的数据总包含一些噪声，这时如果继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵F和单应矩阵H,选择重投影误差比较小的那个作为最终的运动估计矩阵。 ## 6.5 单应矩阵的求解 如果特征点都在一个平面上，即点P满足：
$n^TP+d=0$
那么有：
$-\frac{n^TP}{d}\ =\ 1$
这里的平面，指在_O1_坐标系下的平面，借助这个平面模型，按照推导基础矩阵约束过程类似，在_O2_中对P点观测的齐次坐标为：
$p_2=s_2K\left(RP_1+t\right)$
$=s_2K\left(RP_1+t\left(-\frac{n^TP_1}{d}\right)\right)$
$=s_2K\left(R-\frac{tn^T}{d}\right)P_1$
$=s_2K\left(R-\frac{tn^T}{d}\right){\frac{1}{s_1}K}^{-1}p_1$
使用相机内参进行坐标转换时，如果只有内参K，那么点的坐标为物体归一化坐标到像素坐标，如果带深度或者比例系数s，则为物体实际坐标到像素坐标转换。记p2与p1之间的转换矩阵为H，则有
$p_2=Hp_1$
即：
$\left(\begin{matrix}x_2\\y_2\\1\\\end{matrix}\right)\ =\ \left(\begin{matrix}h_1&h_2&h_3\\h_4&h_5&h_6\\h_7&h_8&h_9\\\end{matrix}\right)\left(\begin{matrix}x_1\\y_1\\1\\\end{matrix}\right)$
由于p2与坐标转换后的p1在同一条射线上，所以等式右边乘以任意非零常数仍然成立。这里可以通过系数调整，使h9为1，于是上述方程，可以整理得如下等式：
$\begin{array}{l} u_{2}=\frac{h_{1} u_{1}+h_{2} v_{1}+h_{3}}{h_{7} u_{1}+h_{8} v_{1}+h_{9}} \\ v_{2}=\frac{h_{4} u_{1}+h_{5} v_{1}+h_{6}}{h_{7} u_{1}+h_{8} v_{1}+h_{9}} \end{array}$
$\begin{array}{l} h_{1} u_{1}+h_{2} v_{1}+h_{3}-h_{7} u_{1} u_{2}-h_{8} v_{1} u_{2}=u_{2} \\ h_{4} u_{1}+h_{5} v_{1}+h_{6}-h_{7} u_{1} v_{2}-h_{8} v_{1} v_{2}=v_{2} \end{array}$
这样一对匹配点，就可以获得两个方程，当有4对匹配点时，则可以得到如下方程组：
$\left(\begin{array}{cccccccc} u_{1}^{1} & v_{1}^{1} & 1 & 0 & 0 & 0 & -u_{1}^{1} u_{2}^{1} & -v_{1}^{1} u_{2}^{1} \\ 0 & 0 & 0 & u_{1}^{1} & v_{1}^{1} & 1 & -u_{1}^{1} v_{2}^{1} & -v_{1}^{1} v_{2}^{1} \\ u_{1}^{2} & v_{1}^{2} & 1 & 0 & 0 & 0 & -u_{1}^{2} u_{2}^{2} & -v_{1}^{2} u_{2}^{2} \\ 0 & 0 & 0 & u_{1}^{2} & v_{1}^{2} & 1 & -u_{1}^{2} v_{2}^{2} & -v_{1}^{2} v_{2}^{2} \\ u_{1}^{3} & v_{1}^{3} & 1 & 0 & 0 & 0 & -u_{1}^{3} u_{2}^{3} & -v_{1}^{3} u_{2}^{3} \\ 0 & 0 & 0 & u_{1}^{3} & v_{1}^{3} & 1 & -u_{1}^{3} v_{2}^{3} & -v_{1}^{3} v_{2}^{3} \\ u_{1}^{4} & v_{1}^{4} & 1 & 0 & 0 & 0 & -u_{1}^{4} u_{2}^{4} & -v_{1}^{4} u_{2}^{4} \\ 0 & 0 & 0 & u_{1}^{4} & v_{1}^{4} & 1 & -u_{1}^{4} v_{2}^{4} & -v_{1}^{4} v_{2}^{4} \end{array}\right)\left(\begin{array}{c} h_{1} \\ h_{2} \\ h_{3} \\ h_{4} \\ h_{5} \\ h_{6} \\ h_{7} \\ h_{8} \end{array}\right)=\left(\begin{array}{c} u_{2}^{1} \\ v_{2}^{1} \\ u_{2}^{2} \\ v_{2}^{2} \\ u_{2}^{3} \\ v_{2}^{3} \\ u_{2}^{4} \\ v_{2}^{4} \end{array}\right)$
求解该非齐次方程组，可以得到H矩阵的每个系数。 ## 6.6 从单应矩阵恢复相机运动 而从H矩阵恢复相机运动，也可以通过奇异值分解的方法，即： 分解的结果会有八组解，此时会把八组解都进行验证，取重投影误差最小的一组，作为最优解。 $\begin{split} &1.d_1=d_2>d_3,d'=\pm d_1=\pm d_2,x_1=x_2=0,x_3=\pm 1 \\ &2.d_1=d_2=d_3,d'=\pm d_1=\pm d_2 =\pm d_3 \\ &3.d_1>d_2=d_3,d'=\pm d_2=\pm d_3,x_2=x_3=0,x_1=\pm 1 \\ &4.d_1>d_2>d_3,d'=\pm d_2,x_2 = 0 \end{split}$
$\begin{equation} \tag{11} \begin{array}{l} H=U\Lambda V^T \\ \Lambda=U^TAV=dU^TRV+(U^Tt)(V^Tn)^T \\ \left\{ \begin{array}{l} R'=sU^TRV \\ t'=U^Tt \\ n'=V^Tn \\ d' =sd \\ s=\det U \det V \end{array} \right. \end{array} \end{equation}$
单应矩阵在相机发生纯旋转时，仍然可以求得旋转，而不是像本质矩阵，在发生纯旋转时，方程其实是失效的，此时求得的矩阵受噪声影响很大，而单应矩阵能更好的应对这个纯旋转问题，来恢复相机的运动。 ## 6.7 三角测量 单张图像是无法得到特征点深度的，在有两张图像后，通过本质矩阵或者单应矩阵，我们可以恢复相机之间的运动。在求得相机运动后，可以通过三角测量，来求得特征点在相机坐标系下的坐标。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1685264984193-b1781b9d-f2cc-488f-b2f0-2dfae821e442.png#averageHue=%23f8f8f8&clientId=ud185277d-3a15-4&from=paste&height=257&id=u38f27af1&originHeight=435&originWidth=816&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=39091&status=done&style=none&taskId=u35787c12-ead2-4b25-ab23-0be3299f28b&title=&width=481.79998779296875)
假设x1,x2是两个特征点（实际物体）的归一化坐标，那么它们满足：
$s_2x_2=s_1Rx_1+t$
其中R与t为O2下O1的位姿。上式两边，同时乘以x2^，可得：
$s_2x_2^\land x_2=s_1x_2^\land Rx_1+x_2^\land t$
显然左式等于0，于是有：
$s_1x_2^\land Rx_1+x_2^\land t=\ 0$
该式中只有s1一个未知数，可以很方便得求出p1的深度。有了p1深度，p2的深度s2也很容易求出了。实际中由于噪声的存在，R,t不一定能使方程准确的等于0，更常见的做法是通过二小二乘的方式求得点的坐标，而不是直接求解。 ## 本章小结 对极几何是求解特征点2D-2D关联时的算法，分为求本质矩阵与单应矩阵。求解完之后，通过BA可以得到特征点的三维坐标，后面的关联就变为3D-2D甚至是3D-3D的数据关联，如何求这两种关联下的相机运动，则在下一节来介绍。 ## 本章思考 1.本质矩阵的自由度为多少？
2.直接法求本质矩阵的过程涉及求解齐次线性方程，而对于齐次线性方程的解，要么只有零解，要么有无穷多个解，这里取哪一个解呢？ ## 附录 ### 1.相机成像模型 根据针孔相机成像原理，相机坐标系下P(X,Y,Z)点投影在像平面上坐标为：
$\left[\begin{matrix}x\\y\\f\\\end{matrix}\right]\ =\frac{1}{Z}\ \left[\begin{matrix}f_x&0&0\\0&f_y&0\\0&0&f\\\end{matrix}\right]\left[\begin{matrix}X\\Y\\Z\\\end{matrix}\right]$
但是一般会省略这个焦距，让其变为1（大多说这里是使用齐次坐标，个人感觉这说不通，齐次坐标是为了矩阵运算的维度相等，加的一个1，但这个是直接替换。个人认为是二维平面上第三个坐标本来没有意义，所以直接省略了）。
然后，转为像素坐标，即在OpenCV中处理的格式：
$\left[\begin{matrix}u\\v\\1\\\end{matrix}\right]\ =\frac{1}{Z}\ \left[\begin{matrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\\\end{matrix}\right]\left[\begin{matrix}X\\Y\\Z\\\end{matrix}\right]$
最后使用归一化坐标，化为最简形式。就是使用P点的投影在距离光心，深度为1m的地方的坐标，来进行像素坐标计算：
$\left[\begin{matrix}u\\v\\1\\\end{matrix}\right]\ =\ \left[\begin{matrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\\\end{matrix}\right]\left[\begin{matrix}X\\Y\\1\\\end{matrix}\right]$ ### 2.坐标系中投影关系的数学描述 引用高翔博士的视觉SLAM十四讲中的表述： > 有时，我们会使用齐次坐标表示像素点。在使用齐次坐标时，一个向量将等于它自身乘上任 > 意的非零常数。这通常用于表达一个投影关系。例如，s1p1和p1成投影关系，它们在齐次坐标 > 的意义下是相等的。我们称这种相等关系为尺度意义下相等(equal up to a scale),记作： > $sp\ \cong\ p$ 个人更喜欢直接使用数值等号来描述，感觉更不容易混淆。那么在坐标系中，两个向量在同一直线上的投影描述就为：
$p_1=sP_2$
P点的归一化坐标与相机_O1_坐标系下坐标P1关系为：
$P_u^1=K^{-1}u_1^齐=s_1P^1$
可得：
$P^1=p_u^1\ast\frac{1}{s_1}$
即P点在相机_O1_坐标系下的归一化坐标，等于像素坐标的齐次转换，同时也等于P在_O1_坐标系下缩放一个系数（由于深度未知）
对于相机_O2_有：
$P_2=RP_1+t\ =\ Rp_u^1\ast\frac{1}{s_1}+t$
对于相机O2下P点的观测，同样的，投影关系：
$P^2=p_u^2\ast\frac{1}{s_2}$
所以有：
$p_u^2\ast\frac{1}{s_2}=Rp_u^1\ast\frac{1}{s_1}+t$ ### 3.本质矩阵性质分析 根据这个等式，可以构建一个E与归一化坐标的方程，E为3x3的矩阵，共有9个量，但实际中，由于t与R各有三个自由度，而t具有尺度等价性，会少一个自由度，所以E的自由度为5，也就是最少可以用5对配对点来求E。 ## 参考 1.视觉SLAM十四讲
[2.对极几何](https://zh.wikipedia.org/wiki/%E5%AF%B9%E6%9E%81%E5%87%A0%E4%BD%95#)
[3.立体视觉中的对极几何——如何更好更快地寻找对应点](https://zhuanlan.zhihu.com/p/158703394) ================================================ FILE: docs/chapter7/chapter7.md ================================================ # **7.积硅步以至千里：前端-视觉里程计之相对位姿估计** 志不强者智不达。——墨翟 --- > **本章主要内容** > 1.已知3D-2D匹配关系，估计相机相对位姿的常用算法 PnP(Perspective-n-Point)算法是求解3D到2D点对运动的方法，它描述了当知道$n$个3D空间点及其投影位置时，如何估计相机的位姿。PnP问题有多种求解方法，例如，用3对点估计位姿的P3P、直接线性变换（DLT）、EPnP（Efficient PnP）等等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，光束法平差（Bundle Adjustment, BA）。 ## 7.1 DLT法 考虑某个空间点$\!\!P\!\!$在世界坐标系下的齐次坐标为$\!\!P_w\!\!=\!\!(X_w,\!Y_w,\!Z_w,\!1)^T\!\!$，在相机坐标系下为$\!\!P_c\!=\!\!(X_c,\!Y_c,\!Z_c)^T\!\!$。在图像$\!I_1\!$中，投影到特征点$\!x_1=(u_1,v_1,1)^T\!$(以归一化平面齐次坐标表示)。定义增广矩阵$\!\![R|t]\!$为一个$\!3\times 4\!$的矩阵，包含了旋转与平移信息。世界坐标与观测的转换关系如下，像素坐标与相机坐标系的转换，深度乘左边比较好表示：
$s_1\left[\begin{matrix}u_1\\v_1\\1\\\end{matrix}\right]=K\left[\begin{matrix}X_c\\Y_c\\Z_c\\\end{matrix}\right]=K\left[\begin{matrix}R&t\\\end{matrix}\right]\left[\begin{matrix}X_w \\ Y_w \\Z_w \\1\\\end{matrix}\right]$
这里设：
$T=K\left[\begin{matrix}R&t\\\end{matrix}\right]=\ \left[\begin{matrix}t_1&t_2&t_3\\t_5&t_6&t_7\\t_9&t_{10}&t_{11}\\\end{matrix}\ \ \ \begin{matrix}t_4\\t_8\\t_{12}\\\end{matrix}\right]$
则原式子为：
$s_1\left[\begin{matrix}u_1\\v_1\\1\\\end{matrix}\right]=T\left[\begin{matrix}X_w \\ Y_w \\Z_w \\1\\\end{matrix}\right]$
如果是求相机$O_1$与相机$O_2$之间的相对关系，则这里的$\!P_w\!$就是$\!P_1\!$，而$R,t$为$O_1$在$O_2$下的姿态。
$s_1\left[\begin{matrix}u_1\\v_1\\1\\\end{matrix}\right]=\ \left[\begin{matrix}t_1&t_2&t_3\\t_5&t_6&t_7\\t_9&t_{10}&t_{11}\\\end{matrix}\ \ \ \begin{matrix}t_4\\t_8\\t_{12}\\\end{matrix}\right]\left[\begin{matrix}X_w \\ Y_w \\Z_w \\1\\\end{matrix}\right]$
简单的矩阵乘法，最后一行可以得到$s_1$的表达式，于是$\!u_1,v_1\!$的值为：
$\begin{array}{c} u_{1}=\frac{t_{1} X_{W}+t_{2} Y_{W}+t_{3} Z_{W}+t_{4}}{t_{9} X_{W}+t_{10} Y_{W}+t_{11} Z_{W}+t_{12}} \end{array}$
$\begin{array}{c} v_{1}=\frac{t_{5} X_{W}+t_{6} Y_{W}+t_{7} Z_{W}+t_{8}}{t_{9} X_{W}+t_{10} Y_{W}+t_{11} Z_{W}+t_{12}} \end{array}$
为简化表示，设：
$t_1=\ \left(t_1\ \begin{matrix}\ t_2&t_3&t_4\\\end{matrix}\right)^T$
$t_2=\ \left(t_5\ \begin{matrix}\ t_6&t_7&t_8\\\end{matrix}\right)^T$
$t_3=\ \left(t_9\ \begin{matrix}\ t_{10}&t_{11}&t_{12}\\\end{matrix}\right)^T$
于是：
$t_1^TP-t_3^TPu_1=0$
$t_2^TP-t_3^TPv_1=0$
其中$t$为待求变量，与求单应矩阵类似，每个点对可以提供两个关于$t$的约束，假设一共有$N$个特征点，则可以得到如下线性方程组：
$\left(\begin{array}{ccc} \boldsymbol{P}_{1}^{T} & 0 & -u_{1} \boldsymbol{P}_{1}^{T} \\ 0 & \boldsymbol{P}_{1}^{T} & -v_{1} \boldsymbol{P}_{1}^{T} \\ \ldots & \ldots & \ldots \\ \boldsymbol{P}_{N}^{T} & 0 & -u_{N} \boldsymbol{P}_{N}^{T} \\ 0 & \boldsymbol{P}_{N}^{T} & -v_{N} \boldsymbol{P}_{N}^{T} \end{array}\right)\left(\begin{array}{c} t_{1} \\ t_{2} \\ t_{3} \end{array}\right)=0$
$\!\!t\!\!$一共有12维，可以通过最少六对匹配点，实现对矩阵T的线性求解，这种方法称为直接线性法（DLT）。如果匹配点数大于6对，可以通过SVD方法对超定方程求最小二乘解。
求得矩阵后，需要先左乘一个内参矩阵的逆$\!\!K^{-1}\!\!$，然后获得$\!\!t\!$与$\!\!t\!\!$。
需要说明的是，这种方法忽略了未知数之间的关联，比如旋转$\!\!R\!\!\in\!\! SO(3)\!\!$，使用这种方式求出的R矩阵不一定满足该关系，其为一个一般性矩阵，需要找一个矩阵对其进行近视，近似的方法由QR分解完成，也可以如下计算：
$R\ \gets\left(RR^T\right)^{\left\{-\frac{1}{2}\right\}}R$
这种解法求得的解，既无尺度，也无约束，要得到尺度及约束，需要构建优化问题。 ## 7.2 P3P 在知道世界坐标系或者其他相机坐标系下三维点与当前相机3个点的对应关系时，最少可以通过3对配对点来求当前相机相对于三维点坐标系的旋转和平移。
P3P需要利用给定的$\!\!\!3\!\!\!$个点的几何关系。它的输入数据为$\!\!\!3\!\!\!$对$\!\!3\text{D}\!\!-\!\!2\text{D}\!\!$匹配点。记$\!\!3\text{D}\!\!$点为$\!\!A\!,\!B\!,\!C\!\!$,$\!\!2\text{D}\!\!$点为$\!\!a,\!b,\!c\!\!$，其中小写字母代表的点为对应大写字母代表的点在相机成像平面上的投影，如下图所示：
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683985374990-677bd69d-f083-4b07-89a2-39040c1a56db.png#averageHue=%23fafafa&id=aQ2hi&originHeight=368&originWidth=482&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=)
P3P算法求得的结果，是另一个坐标系中三维点（比如世界坐标系中的$\!\!A\!,\!B\!,\!C\!\!$），在当前坐标系下的坐标（比如相机成像平面的$\!\!a,\!b,\!c\!\!$对应的相机坐标系下的3D坐标）。从而把一个3D-2D的关联，转化为3D-3D的关联，把PnP问题转换为ICP问题，通过求解ICP的方式获得旋转和平移。需要注意的是，P3P算法最后方程的解有四个，需要额外的一个点来验证，一般会选取重投影误差最小的解作为最优解。
三维点$\!\!A\!,\!B\!,\!C\!\!$，相机成像平面点$\!\!a,\!b,\!c\!$和相机光心$\!O\!$组成的三角形有如下对应关系：
$\Delta O_{ab}-\Delta O_{AB},\quad\Delta O_{bc}-\Delta O_{BC},\quad\Delta O_{ac}-\Delta O_{AC}$
首先回顾一下三角形余弦定理，余弦定理可以看做是勾股定理的推广，表征的是三角形中的边角关系，推导余弦定理可以用勾股定理。含义为对边平方等于夹边平方和减去夹边乘积与对角余弦乘积的2倍。
![](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683985375303-006e318c-1b4c-4083-af5f-51dce5eb3b41.png#averageHue=%2315574a&id=P4GIu&originHeight=336&originWidth=611&originalType=binary&ratio=1&rotation=0&showTitle=false&status=done&style=none&title=)
根据余弦定理，对于这个大的空间三角形，有如下关系：
$\begin{array}{l} O A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle=A B^{2} \\ O B^{2}+O C^{2}-2 O B \cdot O C \cdot \cos \langle b, c\rangle=B C^{2} \\ O A^{2}+O C^{2}-2 O A \cdot O C \cdot \cos \langle a, c\rangle=A C^{2} . \end{array}$
对该方程进行消元，方法是除以$OC^2$，并设$x=OA/OC,y=OB/OC$，则方程变为：
$\begin{array}{l} x^{2}+y^{2}-2 \cdot x \cdot y \cdot \cos \langle a, b\rangle =\frac{A B^{2}}{O C^{2}} \\ x^{2}+1-2 \cdot x \cdot \cos \langle a, c\rangle=\frac{A C^{2}}{O C^{2}} \\ y^{2}+1-2 \cdot y \cdot \cos \langle b, c\rangle=\frac{B C^{2}}{O C^{2}} \end{array}$
然后再进行替换，令：
$u\ =\ \frac{AB^2}{OC^2},\ v=\frac{BC^2}{AB^2},\ w=\frac{AC^2}{AB^2}$
可得：
$\begin{split} &x^2+y^2-2 \cdot x \cdot y \cdot cos = u \\ &x^2 + 1 - 2 \cdot x \cdot cos=wu \\ &y^2 + 1 - 2 \cdot y \cdot cos=uv \end{split}$
将第一个式子，带入第二第三个式子，可得：
$\begin{split} &(1-w)x^2-w \cdot y^2 - 2 \cdot x \cdot cos + 2 \cdot w \cdot x \cdot y \ cdot cos + 1 = 0 \\ &(1-v)y^2-v \cdot x^2 - 2 \cdot y \cdot cos + 2 \cdot v \cdot x \cdot y \ cdot cos + 1 = 0 \end{split}$
因为$\!\!ABC\!\!$之间的相对位置关系是不变的，所以其中$\!v,w\!$值为已知值，而三个角度余弦值可以通过像素观测坐标求得，对于上述方程，只有$\!x,y\!$为未知数，为一个二元二次方程组，求解这个方程组，需要用到吴消元法。
它可以将原方程等效成一组特征列（Characteristic Serial, CS），凡是原方程组的解都会是CS的解，但是CS的解不一定是原方程的解，所以需要验证，这里的等效方程为：
$\begin{cases} a_4 x^4+a_3 x^3 + a_2 x^2 + a_1 x^1 + a_0 = 0 \\ b_1 y - b_0 = 0 \end{cases}$
求得了$\!x,y\!$的值，就可以求取$\!\!OA,\!OB,\!OC\!\!$的值，根据下面的公式，$\!\!AB\!\!$已知，可以先求$\!\!OC\!\!$，然后分别求解$\!\!OB,OA\!$：
$x^2 + y^2 - 2 \cdot x \cdot y \cdot cos=\frac{AB^2}{OC^2}\qquad y=\frac{OB}{OC},x=\frac{OA}{OC}$
求得三个点在相机坐标系下的深度之后，就可以通过像素坐标与三维点的投影关系，求出相机坐标系下点的坐标。
$\bm{A}=\vec{a} \cdot \left \| \bm{PA} \right \|$
附：吴消元法系数
$p=2 \cdot cos \quad q = 2\cdot cos \quad r=2\cdot cos$
$\left\{\begin{aligned} a_{4}= & a^{2}+b^{2}-2 a-2 b+2\left(1-r^{2}\right) b a+1 \\ a_{3}= & -2 q a^{2}-r p b^{2}+4 q a+(2 q+p r) b+\left(r^{2} q-2 q+r p\right) a b-2 q \\ a_{2}= & \left(2+q^{2}\right) a^{2}+\left(p^{2}+r^{2}-2\right) b^{2}-\left(4+2 q^{2}\right) a-\left(p q r+p^{2}\right) b-\left(p q r+r^{2}\right) a b+q^{2}+2 \\ a_{1}= & -2 q a^{2}-r p b^{2}+4 q a+\left(p r+q p^{2}-2 q\right) b+(r p+2 q) a b-2 q \\ a_{0}= & a^{2}+b^{2}-2 a+\left(2-p^{2}\right) b-2 a b+1 \\ b_{1}= & b\left(\left(p^{2}-p q r+r^{2}\right) a+\left(p^{2}-r^{2}\right) b-p^{2}+p q r-r^{2}\right)^{2} \\ b_{0}= & \left.\left((1-a-b) x^{2}+(a-1) q x-a+b+1\right)\right) \\ & \left(\left(r^{3}\left(a^{2}+b^{2}-2 a-2 b+\left(2-r^{2}\right) a b+1\right) x^{3}+\right.\right. \\ & r^{2}\left(p+p a^{2}-2 r q a b+2 r q b-2 r q-2 p a-2 p b+p r^{2} b+4 r q a+q r^{3} a b-2 r q a^{2}+2 p a b+p b^{2}-r^{2} p b^{2}\right) x^{2}+ \\ & \left(r^{5}\left(b^{2}-a b\right)-r^{4} p q b+r^{3}\left(q^{2}-4 a-2 q^{2} a+q^{2} a^{2}+2 a^{2}-2 b^{2}+2\right)+r^{2}\left(4 p q a-2 p q a b+2 p q b-2 p q-2 p q a^{2}\right)+\right. \\ & \left.r\left(p^{2} b^{2}-2 p^{2} b+2 p^{2} a b-2 p^{2} a+p^{2}+p^{2} a^{2}\right)\right) x+ \\ & \left(2 p r^{2}-2 r^{3} q+p^{3}-2 p^{2} q r+p q^{2} r^{2}\right) a^{2}+\left(p^{3}-2 p r^{2}\right) b^{2}+\left(4 q r^{3}-4 p r^{2}-2 p^{3}+4 p^{2} q r-2 p q^{2} r^{2}\right) a+ \\ & \left(-2 q r^{3}+p r^{4}+2 p^{2} q r-2 p^{3}\right) b+\left(2 p^{3}+2 q r^{3}-2 p^{2} q r\right) a b+ \\ & \left.p q^{2} r^{2}-2 p^{2} q r+2 p r^{2}+p^{3}-2 r^{3} q\right) \end{aligned}\right.$ ## 7.3 EPnP EPnP算法最后求解在相机坐标系下，$\!3D\!$点在控制点下坐标时，复杂度为$\!\!O(n)\!\!$，其中$\!\!n\!\!$为配对点对数，比较高效，其名称也是如此得来的。其求解的思路为，先通过世界坐标系或者上一坐标系下3D点的坐标，计算出四个控制点坐标。基于控制点坐标，计算出3D点在控制点坐标系下坐标，而可以证明，在世界坐标系下控制点坐标，与在此时相机坐标系下控制点坐标是相同的。于是问题转为求此时相机坐标系下控制点坐标，此时先假设四个局部坐标系下控制点坐标，通过控制点坐标与观测坐标对应关系，即针孔相机投影，每一对匹配点，可以得到两个方程。把所有配对点都联立方程，得到一个12个未知数，$\!2n\!$个方程的齐次方程组，求解这个方程，方法为求矩阵零特征值的特征向量。由于方程组解系中非共线解向量数量和矩阵的秩与12的差值有关，这里只考虑有1到4个非共线解向量的情况。求得解后，再通过世界坐标系下控制点坐标与局部坐标系下控制点坐标具有相同的相对位置关系，得到解系中系数，从而得到相机坐标系下控制点坐标。这样就把3D-2D的关联，转为3D-3D的关联，最后使用带匹配关系的ICP算法，得到两个坐标系下的R和t。这时候会有四组解，取其中重投影误差最小的一组解为最后的解。
其中求解解系中系数时，可以用高斯牛顿法建立世界坐标系下控制点相对坐标与局部坐标系下相对坐标之差，对系数进一步优化。
所以这些算法，都是线性代数，非线性优化。
下面对该算法进行介绍：
与其他方法相比，EPnP方法的复杂度为O(n)，对于点对数量较多的PnP问题，非常高效。
核心思想是将3D点表示为4个控制点的组合，优化也只针对4个控制点，所以速度很快，在求解 Mx=0时，最多考虑了4个奇异向量，因此精度也很高。
第一步，选择世界坐标系下控制点坐标：
选择3D点的质心，作为第一个控制点，
$\bm{c}_1^w=\frac{1}{n}\sum_{i=1}^{n} \bm{P}_i^w$
对n个3D点进行去重心操作：
$\bm{A}=\begin{bmatrix} (\bm{P}_1^w)^T-(\bm{c}_1^w)^t \\ \cdots \\ (\bm{P}_n^w)^T-(\bm{c}_1^w)^t \end{bmatrix}$
计算A的协方差矩阵，并求其特征值与对应的特征向量，剩余的三个控制点，由这三个特征值与特征向量构成：
$\begin{split} &\bm{c}_2^{w}=\bm{c}_1^{w}+\sqrt{\lambda_1}\bm{V}_1 \\ &\bm{c}_3^{w}=\bm{c}_1^{w}+\sqrt{\lambda_2}\bm{V}_2 \\ &\bm{c}_4^{w}=\bm{c}_1^{w}+\sqrt{\lambda_3}\bm{V}_3 \\ \end{split}$
第二步，求解齐次重心坐标，也就是在上面的控制点坐标系下的坐标：
$\bm{P}_{i}^{w}=\sum_{j=1}^{4}\alpha_{ij}\bm{c}_{j}^{w},\quad with \, \sum_{j=1}^{4}\alpha_{ij}=1$
建立3D点坐标与控制点坐标关系：
$\begin{bmatrix} \bm{P}_{i}^{w} \\ 1 \end{bmatrix} = \bm{C} \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \\ \end{bmatrix} = \begin{bmatrix} \bm{c}_1^{w} & \bm{c}_2^{w} & \bm{c}_3^{w} & \bm{c}_4^{w} \\ 1 & 1 & 1 & 1 \end{bmatrix} \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{bmatrix}$
及：
$\begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{bmatrix}_{4\times1} = \begin{bmatrix} \bm{c}_1^{w} & \bm{c}_2^{w} & \bm{c}_3^{w} & \bm{c}_4^{w} \\ 1 & 1 & 1 & 1 \end{bmatrix}_{4\times 4}^{-1} \begin{bmatrix} \bm{P}_i^w \\ 1 \end{bmatrix}_{4\times1} = \bm{C}^{-1} \begin{bmatrix} \bm{P}_i^w \\ 1 \end{bmatrix}$ 对于世界坐标系与当前相机坐标系下观测点之间转换关系，有：
$\mathbf{c}_{j}^{c}=\left[\begin{array}{ll} \mathbf{R} & \mathbf{t} \end{array}\right]\left[\begin{array}{c} \mathbf{c}_{j}^{w} \\ 1 \end{array}\right] \\$
$\mathbf{p}_{i}^{c}=\left[\begin{array}{ll} \mathbf{R} & \mathbf{t} \end{array}\right]\left[\begin{array}{c} \mathbf{p}_{i}^{w} \\ 1 \end{array}\right]=\left[\begin{array}{ll} \mathbf{R} & \mathbf{t} \end{array}\right]\left[\begin{array}{c} \sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{w} \\ 1 \end{array}\right] \\$
$\mathbf{p}_{i}^{c}=\left[\begin{array}{ll} \mathbf{R} & \mathbf{t} \end{array}\right]\left[\begin{array}{c} \sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{w} \\ \sum_{j=1}^{4} \alpha_{i j} \end{array}\right]=\sum_{j=1}^{4} \alpha_{i j}\left[\begin{array}{ll} \mathbf{R} & \mathbf{t} \end{array}\right]\left[\begin{array}{c} \mathbf{c}_{j}^{w} \\ 1 \end{array}\right]=\sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{c}$
即，相机坐标系下控制点也是由世界坐标系下控制点刚体转换而来的，而控制点的系数不变。 第三步，在相机坐标系下，使用控制点坐标系数，建立观测方程：
$w_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \begin{bmatrix} f_u & 0 & u_c \\ 0 & f_v & v_c \\ 0 & 0 & 1 \end{bmatrix} \sum_{j=1}^{4}\alpha_{ij} \begin{bmatrix} x_j^{c} \\ y_j^c \\ z_j^c \end{bmatrix}$
从而可以得到两个线性方程：
$\$$\begin{aligned} &\sum_{j=1}^{4}\alpha_{ij}f_ux_j^c + \alpha_{ij}(u_c-u_i)z_j^c=0 \\ &\sum_{j=1}^{4}\alpha_{ij}f_vy_j^c+\alpha_{ij}(v_c-v_j)z_j^c=0 \end{aligned}$
把所有n 个点串联起来，我们可以得到一个线性方程组： 上式中，齐次重心坐标，相机内参数和2D像点坐标都是已知量，未知量是4个控制点在相机坐标系下的坐标。共12个位置参数，一个像点可以列2个方程，n 个像点可以列出2n 个方程。
其中$\text{x}=[c_1^{c^T},c_2^{c^T},c_3^{c^T},c_4^{c^T}]$,$\text{x}$就是控制点在摄像头坐标系下的坐标，显然这是一个$12 \times 1$的向量，且$\text{x}$在$M$的右零空间中，或者$\text{x}\in ker(M)$:
$\text{x}=\sum_{i=1}^{N}\beta_i \text{v}_i$
上式中，$\text{v}_i$是$M$的$N$个零特征值对应的$N$特征向量。对于第$i$个控制点：
$\bm{c}_i^c=\sum_{k=1}^N \beta_k \text{v}_k^{[i]}$
求得特征向量后，还需要求解系数β，该步通过世界坐标系下控制点相对坐标与局部坐标系下控制点相对坐标一样来解决：
根据仿真发现$M^TM$为0的特征值的个数喝摄像头的焦距有关（注意：是和焦距有关，而不是和参考点的位置有关！），EPnP算法建议只考虑$N=1,2,3,4$的情况。摄像头的外参描述的只是坐标变换，不会改变控制点之间的距离，$\left \| \bm{c}_i^c - \bm{c}_j^c \right \|^2=\left \| \bm{c}_i^w - \bm{c}_j^w \right \|^2$,
$\left \| \sum_{k=1}^{N}\beta_k \bm{\text{v}}_k^{[i]}-\sum_{k=1}^{N}\beta_k \bm{\text{v}}_k^{[j]} \right\|^2 = \left \| \bm{c}_i^w - \bm{c}_j^w \right \|^2$
这是一个关于$\{\beta_k\}_{k=1,\cdots,N}$的二次方程，这个方程的特点是没有关于$\{\beta\}_{k=1,\cdots,N}$的一次项。如果将这些二次项$\beta_i\beta_j$变量替换为$\beta_{ij}$,那么该方程是$\{\beta_{ij}\}_{i,j=1,\cdots,N}$的线性方程了。对于4个控制点，可以得到$C_{4}^{2}=6$个这样的方程。
如果不挖掘任何附加条件，$N$取不同值的时候，新的线性未知数的个数分别为： - $N=1$，线性未知数的个数为1； - $N=2$，线性未知数的个数为3； - $N=3$，线性未知数的个数为6； - $N=4$，线性未知数的个数为10； $N\!=\!4\!$的时候，未知数的个数多于方程的个数了。注意到$\!\beta_{ab}\beta_{cd}\!=\!\beta_{a}\beta_{b}\beta_{c}\beta_{d}\!=\beta_{a'b'}\beta_{c'd'}\!$,其中$\!\{a',b',c',d'\}\!$是$\{a,b,c,d \}$的一个排列，我们可以减少未知数的个数。举例，如果我们求出了$\beta_{11},\beta_{12},\beta_{13}$,那么我们可以得到$\beta_{23}=\frac{\beta_{12}\beta_{13}}{\beta_{11}}$。这样，即使对于$N=4$，我们也可以求解出$\{\beta_{ij}\}_{i,j=1,\cdots,N}$了。
**高斯-牛顿最优化**
优化的目标函数为：
$\text{Error}(\beta)=\sum_{(i,j)s.t.\,\,i这是一个无约束的非线性最优化问题，简单。
求得系数后，便可以得到世界坐标系下3D点坐标在相机坐标系下坐标，接下来使用icp算法进行求解。
icp算法求解的步骤为：
**Step1：计算点云质心坐标**
$\begin{aligned} &\bm{\text{P}}_c^c=\frac{1}{n}\sum_{i=1}^{n}\bm{P}_i^c \end{aligned}$
$\begin{aligned} &\bm{\text{P}}_c^w=\frac{1}{n}\sum_{i=1}^{n}\bm{P}_i^w \end{aligned}$
**Step2：点云去质心**
$\bm{\bar{\text{P}}}^c= \begin{bmatrix} ({\bm{\text{p}}_1^c})^T-({\bm{\text{p}}_c^c})^T \\ \cdots \\ ({\bm{\text{p}}_n^c})^T-({\bm{\text{p}}_c^c})^T \end{bmatrix}$
$\bm{\bar{\text{P}}}^c= \begin{bmatrix} ({\bm{\text{p}}_1^w})^T-({\bm{\text{p}}_c^w})^T \\ \cdots \\ ({\bm{\text{p}}_n^w})^T-({\bm{\text{p}}_c^w})^T \end{bmatrix}$
**Step3：计算旋转矩阵**$\text{R}$
$\text{W}=\left(\bm{\bar{\text{P}}}^c\right)^T \bm{\bar{\text{P}}}^w$
$[\bm{\text{U}\Sigma \text{V}}]=\text{SVD}(\bm{\text{W}})$
$\bm{\text{R}}=\bm{\text{UV}^T}$
如果$det(\bm{\text{R}})<0,\,\bm{\text{R}}(2,:)=-\bm{\text{R}}(2,:)$
**Step4：计算平移向量t**
$\bm{\text{t}}=\bm{\text{p}}_c^c-\bm{\text{Rp}}_c^w$
注意上面提到R和t有四种情况的解，最后通过计算反投影误差。采用反投影误差最小的求解结果。 ## 7.4 ICP ICP方法是用于求解$\!\!3\text{D}\!-\!3\text{D}\!\!$的位姿估计问题，ICP算法为迭代最近点算法，是求两个点云之间精配准的算法，其流程主要如下： 1. 寻找最近点 2. 计算loss,得到当前使loss最小的旋转和平移 3. 变换原点云，再次寻找最近点 4. 判断此时距离是否小于某个阈值，即收敛，没有则再次回到1进行迭代。 计算的loss函数如下，其中pt,ps为target点云下对应点及source点云下对应点。
$R^\ast,t^\ast=\arg{\min\limits_{R,t} }\frac{1}{{\left|P_s\right|} } \sum_{i=1}^{\left|P_s\right|}{\left|p_t^i-\left(R\cdot p_s^i+t\right)\right|^2}$
第一步，寻找最近点，有如下两种方法：
1.设置距离阈值，当点与点距离小于一定阈值就认为找到了对应点，不用遍历完整个点集；
2.使用 ANN(Approximate Nearest Neighbor) 加速查找，常用的有 KD-tree；KD-tree 建树的计算复杂度为 O(N log(N))，查找通常复杂度为 O(log(N))（最坏情况下 O(N)）。
第二步，寻找最优变换
对于 point-to-point ICP 问题，求最优变换是有闭形式解（closed-form solution）的，可以借助 SVD 分解来计算。
计算两个点云的质心μt, μs
$\mu_p=\frac{1}{n}\sum_{i=1}^{n}p_i \mu_q=\frac{1}{n} \sum_{i=1}^{n}q_i$
对匹配点距离误差进行变换：
$\begin{aligned} &\frac{1}{2}\sum_{i=1}^{n} \left\| \bm{\text{q}}_i-\bm{\text{Rp}}_i- \bm{\text{t}} \right\|^2 \\ =&\frac{1}{2}\sum_{i=1}^{n} \left\| \bm{\text{q}}_i-\bm{\text{Rp}}_i- \bm{\text{t}} - (\mu_q-\bm{\text{R}\mu}_p)+(\bm{\mu}_q-\bm{\text{R}\mu}_p) \right\|^2 \\ =&\frac{1}{2}\sum_{i=1}^{n} \left\| \bm{(\text{q}}_i-\bm{\mu}_q - \bm{\text{R}}(\bm{\text{p}}_i-\bm{\mu_p}))+(\bm{\mu}_q-\bm{\text{R}\mu}_p -\bm{\text{t}}) \right\|^2 \\ =&\frac{1}{2}\sum_{i=1}^{n}\left( \left\| \bm{\text{q}}_i-\bm{\mu}_q - \bm{\text{R}}(\bm{\text{p}}_i-\bm{\mu_p}) \right\|^2+ \left\| \bm{\mu}_q-\bm{\text{R}\mu}_p -\bm{\text{t}} \right\|^2+ 2(\bm{\text{q}}_i-\bm{\mu_q}-\bm{\text{R}}(\bm{\text{p}}_i-\bm{\mu_p}))^T (\bm{\mu_q}-\bm{\text{R}}\bm{\mu_p-t}) \right) \end{aligned}$
对于最后一项，有
$\begin{aligned} &\sum_{i=1}^{n}(\bm{\text{q}}_i-\bm{\mu}_q-\bm{\text{R}}(\bm{\text{p}}_i-\bm{\mu}_p))^T(\bm{\mu}_q-\bm{\text{R}}\bm{\mu}_p-\bm{\text{t}}) \\ =&(\bm{\mu}_q-\bm{\text{R}}\bm{\mu}_p-\bm{t})^T\sum_{i=1}^{n}(\bm{\text{q}}_i-\bm{\mu}_q-\bm{\text{R}}(\bm{\text{p}}_i-\bm{\mu}_p)) \\ =&(\bm{\mu}_q-\bm{\text{R}}\bm{\mu}_p-\bm{t})^T(n\bm{\mu}_q-n\bm{\mu}_q-\bm{\text{R}}(n\bm{\mu}_p-n\bm{\mu}_p)) \\ =&\bm{0} \end{aligned}$
再进一步变换：
设$\bm{\text{p}}_i^{\prime}=\bm{\text{p}}_i-\bm{\mu}_p,\,\bm{\text{q}}_i^{\prime}=\bm{\text{q}}_i-\bm{\mu}_q$，目标函数简化为：
$\frac{1}{2}\sum_{i=1}^{n}\left(\left\| \bm{\text{q}}_i^{\prime}-\bm{\text{R}}\bm{\text{p}}_i^{\prime} \right\|^2+\left\| \bm{\mu}_q-\bm{\text{R}}\bm{\mu}_p-\bm{t} \right\|^2\right)$
设$\bm{\text{R}}^\ast,\bm{\text{t}}^\ast$为最优解，可以将优化变为如下问题： 1. $\bm{\text{R}}^\ast=\argmin_{\bm{\text{R}}}\frac{1}{2}\sum_{i=1}^{n}\left\| \bm{\text{q}}_i^\prime-\bm{\text{R}}\bm{\text{p}}_i^\prime \right\|^2$ 2. $\bm{\text{t}}^\ast = \bm{\mu}_q-\bm{\text{R}}\bm{\mu}_p$ 对于第一步，有：
$\begin{aligned} \bm{\text{R}}^\ast &=\argmin_{\bm{\text{R}}}\frac{1}{2}\sum_{i=1}^{n}\left( {\bm{\text{q}}_i^\prime}^T \bm{\text{q}}_i + {\bm{\text{p}}_i^\prime}^T \bm{\text{R}}^T\bm{\text{R}} \bm{\text{p}}_i^\prime -2{\bm{\text{q}}_i^\prime}^T \bm{\text{R}} \bm{\text{p}}_i^\prime \right) \\ &=\argmin_{\bm{\text{R}}}\frac{1}{2}\sum_{i=1}^{n}\left( {\bm{\text{q}}_i^\prime}^T \bm{\text{q}}_i + \bm{\text{I}}-2{\bm{\text{q}}_i^\prime}^T \bm{\text{R}} \bm{\text{p}}_i^\prime \right) \\ &=\argmin_{\bm{\text{R}}}\sum_{i=1}^{n}-{\bm{\text{q}}_i^\prime}^T \bm{\text{R}} \bm{\text{p}}_i^\prime \end{aligned}$
对这个优化，有如下最优解：
令$\bm{\text{W}}=\sum_{i=1}^{n}\bm{\text{q}}_i^\prime {\bm{\text{p}}_i^\prime}^T$，通过SVD分解有
$\bm{\text{W}}=\bm{\text{U}}\Sigma\bm{\text{V}}^T$
求得旋转和平移为：
当$\bm{\text{W}}$满秩事有：
$\Sigma= \begin{bmatrix} \sigma_1 & 0 & 0 \\ 0 & \sigma_2 & 0 \\ 0 & 0 & \sigma_3 \end{bmatrix}$
且对应唯一的$\bm{\text{U}},\bm{\text{V}}$组合，对应地
$\begin{aligned} &\bm{\text{R}}^\ast=\bm{\text{U}}\bm{\text{V}}^T \\ &\bm{\text{t}}^\ast=\bm{\mu}_q-\bm{\text{R}}\bm{\mu}_p \end{aligned}$
一般需要对求得的旋转矩阵进行检查，即判断其是否为一个旋转矩阵，判断方式如下：
最后还需要进行Orientation rectification，即验证$\bm{R}^\ast=\bm{V}\bm{U}^T$是不是一个旋转矩阵（检查是否有$|R|=1$, 因为存在$|R|=-1$的可能，此时$R$表示的不是旋转而是一个reflection，所以我们还要给上述优化解加上一个$|R|=1$的约束）。
根据矩阵行列式的性质，以及$\bm{\text{U,V}}$都是正交阵：
$\begin{aligned} |\bm{M}|&=|\bm{V}^T||\bm{U}||\bm{R}| \\ &=|\bm{V}^T||\bm{U}|=\pm1 \end{aligned}$
如果$\!|VUT|\!=\!1\!$，则$\!|M|\!=\!1\!$,$\!\bm{R}^\ast\!=\!VU^T\!$已经给出最有旋转：如果$\!|VU^T|\!=\!-1\!$，则$\!|M|\!=\!-1\!$，我们需要求解此时的$\!R\!$，也就是分析$\!M\!$应该具有何种形式。经分析可得出结论：当$\!|M|\!=\!-1\!$时，使得$\!trace(\Sigma M)\!$最大的$M$为：
$M= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix}$
综合考虑$\!|M|\!=\!1\!$和$\!|M|\!=\!-1\!$两种情况，我们可以得到：
$R^\ast=V \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & |VU^T| \end{bmatrix} U^T$
第三步，迭代
每一次迭代我们都会得到当前的最优变换参数$R_k,t_k\!$，然后将该变换作用于当前源点云；"找最近对应点"和"求解最优变换"这两步不停迭代进行，直到满足迭代终止条件，常用的终止条件有：
1.$R_k,t_k\!$的变化量小于一定值
2.loss 变化量小于一定值
3.达到最大迭代次数
迭代结束后，把所有的平移和旋转求和，就是我们要求的平移和旋转了。 ## 本章小结 本章主要介绍了3D-2D中常用算法，最常见的是DLT算法。在EPNP，P3P中，EPNP由于能使用更多点信息，在实际工程中更为常见。ICP则既可以在视觉SLAM中使用，更多的是在激光中使用。 ## 本章思考 1.EPNP算法为什么会更高效？
2.在特征点匹配过程中，不可避免地会遇到误匹配的情况。如果我们把错误匹配输入到PNP或ICP中，会发生怎样的情况？你能想到哪些避免误匹配的方法？ ## 参考 [1.PnP算法详解（超详细公式推导）](https://www.jianshu.com/p/a35fa8ac0803)
[2.【Frobenius norm（弗罗贝尼乌斯-范数）（F-范数）】](https://blog.csdn.net/weixin_45251808/article/details/102968002)
[3.3D-2D：PnP算法原理](https://blog.csdn.net/u014709760/article/details/88029841)
[4.PnP(Perspective-n-Point)问题：各种算法总结分析](https://zhuanlan.zhihu.com/p/399140251)
[5.位姿估计之PnP算法](https://blog.csdn.net/feng_float/article/details/122693009)
[6.相机位姿求解——P3P问题](https://www.cnblogs.com/mafuqiang/p/8302663.html)
[7.[PnP]PnP问题之EPnP解法](https://zhuanlan.zhihu.com/p/59070440)
[8.深入EPnP算法](https://blog.csdn.net/jessecw79/article/details/82945918)
[9.EPnP原理与源码详解](https://zhuanlan.zhihu.com/p/361791835)
[10.三维点云配准之ICP算法](https://wangwenlonggg.github.io/2021/05/26/%25E4%25B8%2589%25E7%25BB%25B4%25E7%2582%25B9%25E4%25BA%2591%25E9%2585%258D%25E5%2587%2586%25E4%25B9%258BICP%25E7%25AE%2597%25E6%25B3%2595/)
[11.三维点云配准 -- ICP 算法原理及推导](https://zhuanlan.zhihu.com/p/104735380) ================================================ FILE: docs/chapter8/chapter8.md ================================================ 在观察的领域中，机遇只偏爱那种有准备的头脑。——巴斯德 --- > **本章主要内容** \ > 1.滤波器，滤波算法简介 \ > 2.卡尔曼滤波公式推导 ## 8.1滤波器及滤波算法 在很久之前，人们刚结束信息传递只能靠信件的时代，通信技术蓬勃发展，无线通信和有线通信走进家家户户，而著名的贝尔实验室就在这个过程做了很多影响深远的研究。为了满足不同电路和系统对信号的需求，比如去除噪声，或者区分不同频率的信号，滤波器就诞生了，而贝尔实验室就是这一领域研究的先行者。早期的滤波器是电子滤波器，是由电阻，电感，电容等电子元件组成的物理电路，其电路图大概长这样：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1700969090586-af3a0fa3-f2d4-428e-86e9-5c11a7be6f46.png#averageHue=%23000000&clientId=u88c33fc0-6828-4&from=paste&height=258&id=ua28c1e98&originHeight=1434&originWidth=2560&originalType=url&ratio=1.25&rotation=0&showTitle=false&size=134358&status=done&style=none&taskId=ue7f15821-e1fe-468c-9cff-57e24519a39&title=&width=461)
这个是由一个电容和一个电阻组成的RC滤波器。而装在实际家电或者设备中的滤波器大概如下图，这个是一个包含高通和低通滤波器的信号分离装置。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1700969007597-71a91932-e0bb-4a9a-b3b9-5ca261c3b7c6.png#averageHue=%23adb8ba&clientId=u88c33fc0-6828-4&from=paste&height=275&id=u72391c8e&originHeight=941&originWidth=1493&originalType=url&ratio=1.25&rotation=0&showTitle=false&size=2664981&status=done&style=none&taskId=ude9dca0a-c7d6-4ccf-9e35-a7bd6f00087&title=&width=437)
随着技术的发展（主要是计算机的兴起），相比于处理原始模拟信号，人们更愿意处理数字信号，这可以带来更高的处理速度，更低的成本和更高的精度，于是，**数字滤波器**诞生了。数字滤波器是对数字信号进行滤波处理以得到期望的响应特性的离散时间系统。这时候数字模拟器还是依靠基本的一些电路元件比如寄存器，延时器，加法器等，但其工作的领域已变为经过数模转换器转化后的数字信号域了，后面广泛用于收音机，蜂窝电话等设备中。
![](https://cdn.nlark.com/yuque/0/2023/jpeg/1782698/1700970067585-286740e8-d640-41e2-b327-793c5c1d79b3.jpeg#averageHue=%232f4b53&clientId=u88c33fc0-6828-4&from=paste&height=259&id=ue88c82c5&originHeight=500&originWidth=900&originalType=url&ratio=1.25&rotation=0&showTitle=false&status=done&style=none&taskId=u8ff7e880-028b-48f3-93dd-ebd2197150e&title=&width=467)
数字滤波器早期主要处理信号，而信号都是一些波形，这也是滤波的由来。后面随着人们对滤波器的扩展，出现了另外一种形式的数字滤波器，其工作过程包含状态空间模型，称为状态空间滤波器，**状态空间滤波器**的一个典型例子是[Rudolf Kalman](https://zh.wikipedia.org/w/index.php?title=Rudolf_Kalman&action=edit&redlink=1)在1960年提出的[卡尔曼滤波器](https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2%E5%99%A8)。这时候虽然没有了波，主要是空间状态，但按传统，这个名称还是保留了下来。
最后，随着计算机的发展，中央处理器（CPU）集成了各种计算单元，所有计算任务都可以交给它，数字滤波器就不用单独保留如寄存器，加法器等元件了，在保留了算法原理和流程之后羽化成仙，成为了**滤波算法**。 ## 8.2 卡尔曼滤波 发明了卡尔曼滤波算法的人，叫做鲁道夫·埃米尔·卡尔曼，是一个匈牙利人，下面是老爷子的近照。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1700971378769-c6a00009-917a-456c-8c5b-a78f83c77bb8.png#averageHue=%234a5154&clientId=u88c33fc0-6828-4&from=paste&height=266&id=ubef5cffa&originHeight=300&originWidth=321&originalType=url&ratio=1.25&rotation=0&showTitle=false&size=144806&status=done&style=none&taskId=ucc8a176f-c34e-4e06-87f1-eaff4d15e9b&title=&width=285)
之所以说是近照，因为卡尔曼老爷子2016年才过世的，算很近代的人物。（莫名想到有人一直以为余华已经不在了的梗）
贴一段wiki中的内容： > 卡尔曼关于滤波的想法最初遭到巨大的怀疑，以至于他被迫在机械工程中首次发表他的成果，而不是在电机工程或系统工程中。然而卡尔曼在1960年访问美国NASA艾姆斯研究中心的史丹利·F·施密特时，在展示他的想法方面取得更大的成功。这使得卡尔曼的滤波在阿波罗计划中得到应用，此外在NASA航天飞机、海军潜艇以及无人驾驶航空器和武器（如巡航导弹）中也得到相关应用。 是的，卡尔曼滤波算法最著名的成就当属在登月工程中的应用，被誉为把人类送上月球的算法。
好了，介绍完卡尔曼滤波算法的基本情况，接下来需要介绍一点背景相关的内容。
**第一：确定性过程与随机过程**
确定性过程可由初始条件准确推导结果，经典物理学基本属于这个范畴，比如我们之前学习的物理过程，由给定初始条件，加速度，算一定时间后的状态，完全由公式可递推出来：
$x_t = x_0 +v_0t+\frac{1}{2}a_0t^2$
但拿到现实中，这个推导容易出现很大偏差，其太过于理想，对实际是不符的。
那么实际过程是什么呢？更准确的描述是一个随机过程，即存在很多不确定性因数和过程在里面。
于是公式变为：
$x_t = x_0 +v_0t+\frac{1}{2}a_0t^2+w \\ P_t = AP_0A^T+Q$
这里的x不再是一个确定的值，而是带着一个分布的随机量，同时结果中也包含随机噪声。这么一对比可以知道，确定性过程其实是随机过程的一个特例，即其概率为1，且不带噪声。
**第二：线性时不变系统(有点累，后面再补)**
用一句话概括时不变系统：如果你的系统是时不变系统，如果将相同的输入输入到系统中，无论何时将输入输入到系统中，都将获得相同的输出。
**第三：推导的一般假设（有点累，后面再补）**
零均值假设：
马尔科夫假设：
接下来是卡尔曼滤波的简介及五个经典公式： > 卡尔曼滤波通常用来估计带噪信号中隐藏的真实信息。它的输入信号带有“过程噪声”和“观测噪声”，输出信号是对真实信号的最优估计，处理的本质是对噪声的滤除。 > 还可以认为卡尔曼滤波是一个估计器：每一时刻，它迭代式的对隐藏信息进行预测，再根据观测值进行修正，最后给出最优估计。 > 最后，还可以认为卡尔曼滤波是一系列数学方程的总称，如下表所示，经典的卡尔曼滤波算法由五个公式组成，希望你在读完本文后，可以对这五个公式有了深刻的理解。 > $\begin{aligned} & \tilde{\mathbf{x}}_t^{-}=\mathbf{F} \tilde{\mathbf{x}}_{t-1}+\mathbf{B u} \\ & K_t=\frac{P_t^{-} H^T}{H P_t^{-} H^T+R} \\ & \tilde{\mathbf{x}}_t=\tilde{\mathbf{x}}_t^{-}+\mathbf{K}_{\mathbf{t}}\left(\mathbf{z}_{\mathbf{t}}-\mathbf{H} \tilde{\mathbf{x}}_t^{-}\right) \\ & P_t=\left(I-K_t H\right) P_t^{-} \\ & \mathbf{P}_{t+1}^{-}=\mathbf{F} \mathbf{P}_t \mathbf{F}^T+\mathbf{Q}_{\mathbf{t}+\mathbf{1}} \end{aligned}$ ## 8.3 卡尔曼滤波算法推导简明版 卡尔曼滤波是基于马尔科夫假设的，即下一时刻状态只与上一时刻有关。其主要针对于线性高斯系统，计算的流程如下：
假设状态$\mathbf{X}$的转移方程为：
$\begin{equation} \begin{aligned} \hat{\mathbf{x}}_{k} & =\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} \\ \mathbf{P}_{k} & =\mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T} \end{aligned} \end{equation}$
其中$\mathbf{F}_{k}$为状态转移矩阵，而$\mathbf{P}_{k}$为$\mathbf{X}_{k}$的方差
$\mathbf{X}$只表示自身的状态，另外一些系统还会有外部控制因数，比如火车减速时，速度是状态量，但可以有刹车装置进行减速，如果系统存在这部分控制因数，需要把这部分加到状态转移中：
$\begin{aligned} \hat{\mathbf{x}}_{k} & =\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} +Bu_k\\ \mathbf{P}_{k} & =\mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T} \end{aligned}$
另外，$\mathbf{X}$中包含环境的一些未知变量，我们假设为噪声，同时噪声分布假设服从高斯分布，于是有如下方程：
$\begin{aligned} \hat{\mathbf{x}}_{k} & =\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} +Bu_k +w_k\\ \mathbf{P}_{k} & =\mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T} +R \end{aligned}$
卡尔曼滤波中，还需要使用观测方程来更新，一般观测方程是需要从状态变为观测量的，即需要有一个观测量到状态的转换，但很多时候这个转换方程都没有，但这并不影响我们假设，如果没有转换方程，到时候直接把转换矩阵设为单位矩阵就行，那现在假设观测方程为：
$z_k = H*x_k+v_k$
其中$H$为观测方程，$v_k$为观测的噪声分布，假设其服从$v_k\sim N\left(0,Q\right)$，即零均值，方差为Q的高斯分布：
到这里，需要说明一点，上一次滤波的结果，会作为下一次滤波的初始值，即由上一次后验概率，通过状态转移矩阵与控制向量，变为目前的先验值，所以原来的观测方程需要变为：
$\begin{aligned} \hat{\mathbf{x}}_{\bar{k}} & =\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} +Bu_k +w_k\\ \mathbf{P}_{\bar{k}} & =\mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T} +R \end{aligned}$
其中下标不带横杠的，表示后验值，带横杠的，代表先验值，这样就由上一时刻的最优估计，得到了当前预测的先验状态及先验方差。
对于观测方程，其方差主要由观测方差决定，即：
$P_{z_k} = R$
zk的均值为H*xk,即zk服从N(H*xk,R)的分布。
这两个分布都是高斯分布，一个是状态的先验分布，一个是传感器测量的分布，这个测量与xk的状态也有关，现在要求这两个分布的联合分布，并求其最大值，很简单，把两个分布乘起来，由于高斯分布的乘积，还是高斯分布，取其均值处，就是概率最大的状态。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684340645013-e837abfe-6084-4761-9ec1-7511a7736935.png#averageHue=%23fdfdfd&clientId=ud705f196-c300-4&from=paste&height=353&id=ub773d1ce&originHeight=441&originWidth=720&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=59646&status=done&style=none&taskId=u8bd53790-1012-4600-bae7-ceada7e80d0&title=&width=576)
乘积的部分，就是所说的状态更新，首先由高斯分布的公式可得：
$\mathcal{N}(x, \mu, \sigma)=\frac{1}{\sigma \sqrt{2 \pi}} e^{-\frac{(x-\mu)^{2}}{2 \sigma^{2}}}$
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684340775920-336bcbe7-4fef-42d0-a4f9-382847769489.png#averageHue=%23ecece5&clientId=ud705f196-c300-4&from=paste&height=65&id=ub2acad42&originHeight=81&originWidth=370&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=20046&status=done&style=none&taskId=u43293ced-253c-4f7d-9f21-f6e9b80d8a8&title=&width=296)
那么两个分布相乘，系数结果为：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684340792537-69fa9ec7-7ec5-4eec-bfdc-8136aa44b775.png#averageHue=%23edece7&clientId=ud705f196-c300-4&from=paste&height=135&id=uf7b2cb96&originHeight=169&originWidth=221&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=26016&status=done&style=none&taskId=u1fd35b68-9970-40b4-ae11-b28f88ce9c0&title=&width=176.8)
其中均值u就是概率最大时的值，σ就是方差，其中这个式子就是最后我们要求的，但这个式子有点复杂，于是用一个系数化简为：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684340927621-51d076b6-bd29-4fcd-9196-dc197d120eec.png#averageHue=%23f0efea&clientId=ud705f196-c300-4&from=paste&height=146&id=u45252e44&originHeight=182&originWidth=203&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=24884&status=done&style=none&taskId=udebdd01d-a3a3-454c-ba0c-ae10cd15303&title=&width=162.4)
其中k称为卡尔曼增益，u0为预测值，u1为观测。可以看到，k为0到1之间的数，分子为预测的方差，如果预测方差越大，则越向观测值靠拢，如果预测方差越小，则越向预测值靠拢。
截至目前，我们有用矩阵$\left(\mu_0, \Sigma_0\right)=\left(H_k \hat{x}_k, H_k P_k H_k^T\right)$预测的分布，有用传感器读数$\left(\mu_1, \Sigma_1\right)=\left(\vec{z}_k, R_k\right)$预测的分布。把它们代入上节的矩阵等式中：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684341570383-5015b365-70f6-425c-a3ed-64c03ba69afe.png#averageHue=%23fafaf9&clientId=ud705f196-c300-4&from=paste&height=135&id=u5107c633&originHeight=329&originWidth=771&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=68732&status=done&style=none&taskId=u34f92720-17ab-4f0a-958d-21bb8baa301&title=&width=316)
相应的，卡尔曼增益就是：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684341570383-5015b365-70f6-425c-a3ed-64c03ba69afe.png#averageHue=%23fafaf9&clientId=ud705f196-c300-4&from=paste&height=110&id=wM391&originHeight=329&originWidth=771&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=68732&status=done&style=none&taskId=u34f92720-17ab-4f0a-958d-21bb8baa301&title=&width=258) 两个式子左边都有不少Hk矩阵，同时把这个矩阵去掉，则K变为：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684341669218-6197f9b4-c149-4762-a3c1-c1f11bc58158.png#averageHue=%23eff3e6&clientId=ud705f196-c300-4&from=paste&height=128&id=u0db3976f&originHeight=160&originWidth=313&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=31158&status=done&style=none&taskId=u5aeeae1e-f537-4732-980b-6114a1df822&title=&width=250.4)
于是，我们得到最后卡尔曼更新的公式：
$\begin{array}{c} K_{k}=\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T}+R} \\ \hat{x}_{k}=\hat{x}_{\bar{k}}+K_{k}\left(z_{k}-H \hat{x}_{\bar{k}}\right) \\ P_{k}=\left(I-K_{k} H\right) P_{\bar{k}} \end{array}$
其中计算K的都是使用先验方差，R为传感器方差。
Zk为实际观测值，Hxk为预测的观测值
最后使用K及先验方差，得到后验方差及后验均值。
卡尔曼滤波更新结束，是不是很简单。 ## 8.4 卡尔曼滤波算法推导本质版 卡尔曼滤波属于贝叶斯滤波的一种，本质是通过先验和观测，获得最大后验概率。而最优的后验概率，指真实值与估计值的协方差最小，即下面这个公式：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1700978512899-c2bd6506-62bf-4468-817a-e0bd44320e34.png#averageHue=%23f4f4f4&clientId=u88c33fc0-6828-4&from=paste&height=33&id=u97bf98c6&originHeight=41&originWidth=238&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=2522&status=done&style=none&taskId=uc96962c3-07b2-4597-a790-c4f6b7156cd&title=&width=190.4)
其中前一个x为真实值，后一个x为卡尔曼最终的估计值。
而这里的最终估计值，是在预测值的基础之上，使用观测值进行更新，更新的方式是基于预测和观测之间的误差
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1700978713834-9a0d6e21-43be-4253-9c05-6b3511ef3651.png#averageHue=%23f8f8f8&clientId=u88c33fc0-6828-4&from=paste&height=50&id=u74061e4a&originHeight=62&originWidth=396&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=4452&status=done&style=none&taskId=ua65f531e-3510-4c97-9169-331d3c14337&title=&width=316.8)
而这个误差，就是卡尔曼增益。
这里我们统一把真实值与最终预测值的协方差放在k+1步来做，其实k+1与k都可以，只要理解其为递推后的一步就行。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1700978839102-87d0814a-c697-44c4-b7aa-f7454c9bbea4.png#averageHue=%23f5f5f5&clientId=u88c33fc0-6828-4&from=paste&height=133&id=ubbea8f19&originHeight=166&originWidth=594&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=24943&status=done&style=none&taskId=u00ffe45f-47fd-421f-b086-67bd5c3579d&title=&width=475.2)
方差主要是矩阵对角线上的元素，那么这里为求最小方差，可以只对协方差矩阵对角线上元素求导，即对迹求导就行。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1700979010353-1c0ea0c3-947f-4d1c-b9c5-c82c87deb6cc.png#averageHue=%23f7f7f7&clientId=u88c33fc0-6828-4&from=paste&height=50&id=ufb99dc11&originHeight=62&originWidth=441&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=6022&status=done&style=none&taskId=u3c49fd92-18cd-42bf-b54c-cca0d1b60ad&title=&width=352.8)
最终得到，卡尔曼最优增益为：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1700979024166-162a7915-0357-479e-874a-d6ef04bb75b8.png#averageHue=%23f5f5f5&clientId=u88c33fc0-6828-4&from=paste&height=38&id=u827ff0e9&originHeight=48&originWidth=237&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=3365&status=done&style=none&taskId=uaea90866-391e-48be-9e95-11e77d14718&title=&width=189.6)
把W带回P(k+1|k+1)，得：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1700979100956-6c2e739a-3ef3-4ea9-877d-728000905d71.png#averageHue=%23f2f2f2&clientId=u88c33fc0-6828-4&from=paste&height=28&id=ueeb10184&originHeight=35&originWidth=326&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=4117&status=done&style=none&taskId=uadce11c2-128d-4a41-82f6-64d6433b664&title=&width=260.8)
和原来经典五公式中3,4,5公式就是一致的了。
更详细的推导，见链接：[卡尔曼滤波-wiki](https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2#%E6%8E%A8%E5%AF%BC%E5%90%8E%E9%AA%8C%E5%8D%8F%E6%96%B9%E5%B7%AE%E7%9F%A9%E9%98%B5) ，[卡尔曼滤波最完整公式推导](https://zhuanlan.zhihu.com/p/341440139) ## 本章小结 本章主要介绍什么是卡尔曼滤波，及其算法过程。 ## 本章思考 1.卡尔曼滤波适用于什么系统？
2.卡尔曼增益的含义是什么？ ## 参考 [1.图说卡尔曼滤波，一份通俗易懂的教程](https://zhuanlan.zhihu.com/p/39912633)
[2.卡尔曼滤波五个公式各个参数的意义](https://blog.csdn.net/wccsu1994/article/details/84643221)
[3.卡尔曼滤波（Kalman Filter）原理与公式推导](https://zhuanlan.zhihu.com/p/48876718)
[4.卡尔曼滤波算法的五大核心公式含义](https://blog.csdn.net/qq_36812406/article/details/127821768?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_baidulandingword~default-0-127821768-blog-84643221.235^v36^pc_relevant_default_base&spm=1001.2101.3001.4242.1&utm_relevant_index=3)
[5.一文看懂卡尔曼滤波（附全网最详细公式推导和Matlab代码）](https://zhuanlan.zhihu.com/p/559191083)
[6.卡尔曼滤波-wiki](https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2#%E6%8E%A8%E5%AF%BC%E5%90%8E%E9%AA%8C%E5%8D%8F%E6%96%B9%E5%B7%AE%E7%9F%A9%E9%98%B5)
[7.卡尔曼滤波最完整公式推导](https://zhuanlan.zhihu.com/p/341440139)
[8.电子滤波器](https://zh.wikipedia.org/wiki/%E7%94%B5%E5%AD%90%E6%BB%A4%E6%B3%A2%E5%99%A8)
[9.数字滤波器](https://zh.wikipedia.org/wiki/%E6%95%B0%E5%AD%97%E6%BB%A4%E6%B3%A2%E5%99%A8)
[9.鲁道夫·卡尔曼](https://zh.wikipedia.org/zh-cn/%E9%B2%81%E9%81%93%E5%A4%AB%C2%B7%E5%8D%A1%E5%B0%94%E6%9B%BC) ================================================ FILE: docs/chapter9/chapter9.md ================================================ # **9.每次更好，就是最好：后端之非线性优化** 人生如同故事。重要的并不在有多长，而是在有多好。——塞涅卡 --- > **本章主要内容** \ > 1.最小二乘问题 \ > 2.牛顿法 \ > 3.高斯牛顿法 \ > 4.LM法 \ > 5.BA 在后端优化中，存在着两种优化算法，一种是利用矩阵分解求得最小二乘问题解的方法，不用迭代，这种我称之为线性代数的方法。第二种就是利用迭代的方式，不断调整参数，使残差最小，这种算法是本章要介绍的算法，也是我们通常所说的非线性优化的方法。 ## 9.1 非线性优化 这里有类似于机器学习中监督学习的概念，使用预测值与观测值构建出一个残差，而状态值与环境观测则是需要调节的变量。把所有残差求和，迭代更新变量值，使loss不断下降，到一个最小值时，这时的状态量就被称之为最优估计值。
这里的loss，一般会构建为观测与预测的差值的平方，所以构建的问题一般为最小二乘问题。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1684572172464-9a531ea9-9ea8-4ccf-a694-93a20b08a6bd.png#averageHue=%23f6f6f6&clientId=ue566fc9f-7bc8-4&from=paste&height=304&id=RHGRm&originHeight=380&originWidth=619&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=76876&status=done&style=none&taskId=u7c1f007a-17c1-4a39-b8b2-a379063d2aa&title=&width=495.2) ## 9.2牛顿法 牛顿法来源于求方程解，但求极值的过程，也是求函数导数为0的过程，所以二者本质上是一样。记残差函数为：
$F(x) = \frac {1}{2}||f(x)||^2$
对F(x)进行二阶泰勒展开
$F(x) = F(x_0) + F^{'}(x_0)(x-x_0)+\frac{1}{2}F^{''}(x_0)(x-x_0)^2 +\sigma^2(x-x_0)^2$
对F(x)的泰勒展开求导：
$F^{'}(x) = F^{'}(x_0)+F^{''}(x_0)(x-x_0)$
令导数为0，得到
$F^{''}(x_0)(x-x_0) = -F^{'}(x_0)$
将$x-x_0$视作$x_0$基础上的增量$\Delta x$，则$x = x_0 +\Delta x$，于是原方程变为
$F^{''}(x_0)\Delta x = -F^{'}(x_0)$
对于变量为向量的情况，这里的一阶导数与二阶导数，就变为雅克比矩阵J与海森矩阵H。上式可变为：
$H(x_0)\Delta x = -J(x_0)$
求得增量$\Delta x$后，更新$x_0$，在计算新x下loss，重复求增量，不断迭代直到收敛，最后得到的x就是我们要求的最优变量。
上面的方程也称为增量方程。 ## 9.3 高斯牛顿法 对于复杂函数，特别2023是非线性函数，求二阶导数十分困难，甚至是难以求出的，牛顿法中直接求海森矩阵的做法十分困难，因此出现了高斯牛顿法：
具体做法是对f(x)做一阶泰勒展开，原loss函数变为
$\begin{array}{l} F(x) = \frac {1}{2}||f(x)||^2 = \frac {1}{2}||f(x_0)+f^{'}(x_0)(x-x_0)||^2 \\ =\frac{1}{2}\left(\|f(x_0)\|^{2}+2 f(x_0) f^{'}(x_0) (x-x_0)+ f^{'}(x_0)^2(x-x_0)^2 \right) \end{array}$
对上式关于x求导，得：
$\begin{array}{l} F^{'}(x)= f(x_0) f^{'}(x_0) + f^{'}(x_0)^2(x-x_0) \end{array}$
令导数等于0，有：
$f^{'}(x_0)^2(x-x_0) = -f(x_0) f^{'}(x_0)$
同样的，令$x-x_0$为$\Delta x$，对于多元函数，一阶导为雅克比矩阵，于是原方程变为：
$J(x_0)J(x_0)^{T} \Delta x = -f(x_0)J(x_0)$
同样的方法，求出增量x后，更新x，得到当前loss，不断迭代下去直到收敛。
对比于牛顿法的增量方程，可以看到高斯牛顿法左边用更容易求导的雅克比矩阵的乘积，近似代替了海森矩阵。右侧则多乘了一个$f(x_0)$
$H(x_0)\Delta x = -J(x_0)$ ## 9.4 列文伯格-马夸而特法 在求增量方程的解时，涉及矩阵求逆，如下：
$\Delta x = -H(x_0)^{-1}J(x_0)$ 在高斯牛顿法中,$H(x_0)$对应$J(x_0)J(x_0)^T$
但是，对于高斯牛顿法，并不能保证$H(x_0)$可逆，另外，如果增量$\Delta x$过大，线性近似的误差也会很大，为了解决这两种问题，出现了列文伯格-马夸而特法，也就是LM法。
LM法是对高斯牛顿法（GN法）的改进，通过引入信赖区域的残差，对近似范围进行了限制，其中的残差为：
$\min \frac{1}{2}\left\|f\left(x_k\right)+J\left(x_k\right)^T \Delta x_k\right\|^2, \quad \text { s.t } \quad\left\|D \Delta x_k\right\|^2 \leq \mu$
增量方程的获取，构建拉格朗日函数，$\lambda$是系数因子：
$L(\Delta x, \lambda)=\frac{1}{2}\left\|f(x)+J(x)^T \Delta x\right\|^2+\frac{\lambda}{2}\left(\|D \Delta x\|^2-\mu\right)$
这样的话，化简后求导就可以得到：
$J(x) f(x)+J(x) J^T(x) \Delta x+\lambda D^T D \Delta x=0$
我们化简后得到：
$\left(J J^T+\lambda D^T D\right) \Delta x=-J f$
在本文中，我们令$H=J J^T, g=-J f$。在实际使用中，通常用$I$来代替$D^TD$。所以，公式就变为：
$(H+\lambda I) \Delta x_k=g$
这样就得到了增量方程。
LM称为置信区域法，其会对线性近似程度的好坏进行度量，通过如下系数计算：
$\rho=\frac{f(x+\Delta x)-f(x)}{J(x)^{T} \Delta x}$
该分数代表近似的准确性，在一定范围内，会认为近似是有效的，因此LM法也称为基于区域搜索的方法，及信赖区域法，可以看到其分为以下几种情况： - ρ 接近1，近似是好的，不需要更改； - ρ 太小，则实际减少的值小于近似减少的值，近似较大，需要缩小近似的范围； - ρ 太大，则实际减少的值大于近似减少的值，近似较小，需要扩大近似的范围。 根据ρ的值，来进行步长的动态调整，即改变增量方程中的λ值。- ρ 越大，表明 L对F的近似效果越好，所以可以增大 λ以使得LM法接近梯度下降法
相反，ρ 越小，表明 L的近似越差，所以应该减小μ以使得迭代公式接近高斯牛顿法。
由于系数矩阵中加入了λ乘以单位矩阵，可以保证系数矩阵的正定可逆。其次，通过改变λ值，可以起到调整步长的作用，当线性近似程度不好时，减小更新的步长，可以有更好的收敛性。 ## 9.5 BA BA是VSLAM里广泛使用的技术，它通过同时优化地图点与相机位姿，使空间3维点与相机平面观测的误差最小，每个3D点光束都能穿过观测相机的光心。BA是VSLAM里常用的后端算法，但由于其灵活性，也可以在前端使用。相比于上面几种线性代数的方法，BA一方面是非线性方法，可以最大程度使用所有匹配点数据，另一方面可以优化三维点坐标，被称为slam中万金油的方法。
BA不仅可以求两帧之间的位移，还可以求多帧的姿态。如果固定住路标点，只优化位姿，这种BA叫做运动BA。而把路标点与姿态一起优化叫做完整BA。由于优化变量数量庞大，完整BA优化比较耗时。
BA优化时需要给一个迭代的初始值，一般做法是不直接进行BA，而是**先用前面介绍的PnP算法或者对极几何先获得相机位姿及3D点坐标初始值，然后把初始值交给BA做最终的优化调整**。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683995376650-0bd3f787-1ea0-49e9-b992-e98e8a2404dd.png#averageHue=%23f2efee&clientId=u2865029d-01ae-4&from=paste&height=397&id=uc539b179&originHeight=496&originWidth=692&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=269226&status=done&style=none&taskId=u8a2a4d6a-3c26-4b96-a75c-f42f39889a4&title=&width=553.6) > BA算法步骤主要如下： > 1.构建BA残差 > 2.构建增量线性方程 > 3.利用增量线性方程系数矩阵的稀疏性，进行边缘化，求解出位姿增量 > 4.再求3D点增量 > 5.更新变量 > 6.判断是否收敛，没有的话则返回1继续迭代计算 下面进行详细介绍：
**第一步，构建残差**
BA残差为特征点观测值与预测值之差的平方，其中观测值为像素坐标系下特征点坐标，预测值为3D点在该相机坐标系下的像素平面投影坐标。这里需要把3D点转换到当前相机坐标系，假设3D点为点P，相机相对旋转与平移为R,t
$P^{\prime} = Rp+t$
P'在归一化平面上坐标为：
$P_c = [u_c,v_c,1] = [P^{\prime}_x/P^{\prime}_z,P^{\prime}_y/P^{\prime}_z,1]$
通过相机镜头时，存在畸变，则畸变坐标为：
$\left\{\begin{array}{l} u_{\mathrm{c}}^{\prime}=u_{\mathrm{c}}\left(1+k_{1} r_{\mathrm{c}}^{2}+k_{2} r_{\mathrm{c}}^{4}\right) \\ v_{\mathrm{c}}^{\prime}=v_{\mathrm{c}}\left(1+k_{1} r_{\mathrm{c}}^{2}+k_{2} r_{\mathrm{c}}^{4}\right) \end{array}\right.$
根据相机模型，最后像素坐标为：
$\left\{\begin{array}{l} u_{\mathrm{s}}= f_x*u_{\mathrm{c}}^{\prime} + c_x \\ v_{\mathrm{s}}= f_y*v_{\mathrm{c}}^{\prime} + c_y \end{array}\right.$
以上，从原始3D点，到观测点的转换，称为观测模型，这里记为：
$z = h(T,p)$
其中T为包含旋转与平移的转换矩阵。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683992427303-97b0dc6d-5374-49ec-b1b8-6e27f91d8460.png#averageHue=%23f6f6f6&clientId=u55127b8a-7d04-4&from=paste&height=199&id=Blf1p&originHeight=249&originWidth=1016&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=87687&status=done&style=none&taskId=uf68d77ca-c8d7-48c3-b834-b1a6bceb3f6&title=&width=812.8)
转换过程如上图：
现在，设相机平面观测像素坐标为z',则观测误差为：
$e = z^{\prime} - h(T,p)$
将所有点的观测残差平方求和，得到如下观测残差：
$\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}\left\|\bm{e}_{ij} \right\|^2= \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}\left\| \bm{z}_{ij}-h(T_i,p_j)\right\|^2.$
该残差为最小二乘残差，对其求最小化的过程，同时对位姿与3D点进行了调整，就是所谓的BA，到此，BA残差构建完毕！
可以看到，这个残差也是通过3D-2D点的关联完成的。 **第二步，构建增量方程**
将所有待求变量，写为一个向量形式为：
$\bm{x}=[T_1,\cdots,T_m,p1,\cdots,p_n]^\text{T}.$
向量中$\!T\!\!$表示相机位姿，即旋转与平移，$\!p\!$表示3D点坐标，如果是求两帧之间相对位移，则其中$\!T\!$就只有$\!T_1\!$。假设残差函数为$\!f(x)\!$，则当我们在$x$上加一个增量$\Delta_x$时，$\!f(x)\!$的变化为：
$\frac{1}{2}\| f(x+\Delta x) \|^2 \approx \frac{1}{2}\sum_{i=1}^{m}\sum_{i=1}^{n} \| e_{ij}+F_{ij}\Delta\xi_i+E_{ij}\Delta p_j \|^2$
其中，$F$表示整个代价函数在当前状态下对相机姿态的偏导数，而$E$表示该函数对路标点位置的偏导。以上是对每一个点进行求和，现在把每个点及每个姿态，各自放在一起，形成一个向量，则上述式子变为：
$\frac{1}{2}\| f(x+\Delta x) \|^2 = \frac{1}{2} \| e+F\Delta x_c+E\Delta x_p \|^2$
对该函数求最小值，需要用到非线性优化中的最小二乘法，不对其进行展开讨论，只说明一般形式，不管选择何种最小二乘中的高斯牛顿还是LM法，最后都需要求增量方程，该方程由上式求导数为0得来：
$\bm{H}\Delta \bm{x}=\bm{g}.$
如果是高斯牛顿法，则增量方程如下：
$J^T(x_0)J(x_0)\Delta x=-J^T(x_0)f(x_0)$
高斯牛顿法和列文伯格一马夸尔特方法的主要差别在于，这里的$H$是取$J^TJ$还是$J^TJ+\lambda I$的形式。由于把变量归类成了位姿和空间点两种，所以雅可比矩阵可以分块为
$J=[F\,\, E]$
那么以高斯牛顿法为例，$H$矩阵为如下形式：
$H=J^TJ= \begin{bmatrix} F^TF & F^TE \\ E^TF & E^TE \end{bmatrix}$
因为该方法包含了所有变量，一般特征点有几百个，这个$H$矩阵会非常大，在求增量时，如果直接求矩阵的逆，其运算复杂度为$O(n^3)$，计算会非常耗时，但幸运的是，$H$矩阵本身具有稀疏性，通过这个性质，可以加速增量方程的求解。 第三步，通过边缘化求解增量方程：
$H$矩阵的稀疏性是由雅可比矩阵$J(x)$引起的。考虑这些代价函数当中的其中一个$e\!$。注意，这个误差项只描述了在$T$看到$P$这件事，只涉及第$\!i\!$个相机位姿和第$\!j\!$个路标点，对其余部分的变量的导数都为0。所以该误差项对应的雅可比矩阵有下面的形式：
$J_{ij}(x)=\left( \bm{0}_{2\times6},...,\bm{0}_{2\times6},\frac{\partial \bm{e}_{ij}}{\partial \bm{T}_i}, \bm{0}_{2\times6},...,\bm{0}_{2\times3},...,\bm{0}_{2\times3},\frac{\partial \bm{e}_{ij}}{\partial \bm{p}_j}, \bm{0}_{2\times3},...,\bm{0}_{2\times3} \right).$
其中$\!\bm{0}_{2\times 6}\!$表示维度为$\!2\!\times\!6\!$的$\!\bm{0}\!$矩阵，同理，$\!\bm{0}_{2\times 3}\!$也是一样的。该误差项对相机姿态的偏导维度为$\!2\!\times\!6\!$，对路标点的偏导维度是$\!2\!\times\!3\!$。这个误差项的雅可比矩阵，除了这两处为非零块，其余地方都为零。这体现了该误差项与其他路标和轨迹无关的特性。
以图9-3为例，我们设$\!J_{ij}\!$只在$\!i\!,\!j\!$处有非零块，那么它对$\!H\!$的贡献为$\!\!J_{ij}^TJ_{ij}\!$，具有图上所画的稀疏形式。这个$\!J_{ij}^TJ_{ij}\!$矩阵也仅有4个非零块，位于$\!(i,i),\!(i,j),\!(j,i),\!(j,j)\!$。对于整体的$\!H\!$，有
$\bm{H}=\sum_{i,j}\bm{J}_{ij}^{T}\bm{J}_{ij},$
请注意，$\!i\!$在所有相机位姿中取值，$\!j\!$在所有路标点中取值。我们把$\!H\!$进行分块：
$H= \begin{bmatrix} H_{11} & H_{12} \\ H_{21} & H_{22} \end{bmatrix}.$
则J的稀疏性对最后$\!H\!$矩阵稀疏性的影响如下：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683994394256-8a828950-419e-4327-b7d1-29be5078704a.png#averageHue=%23f5f5f5&clientId=u55127b8a-7d04-4&from=paste&height=221&id=u04e9227c&originHeight=276&originWidth=699&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=50713&status=done&style=none&taskId=u6ed8b425-2cdc-43be-8c89-9235d3912c8&title=&width=559.2)
则对原$H$矩阵，可以做如下划分：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1683994554810-e2e324bd-127c-48df-890c-7fa730ff78ea.png#averageHue=%23bababa&clientId=u55127b8a-7d04-4&from=paste&height=340&id=u60058352&originHeight=425&originWidth=433&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=144497&status=done&style=none&taskId=uf54266e3-1e6c-4565-8e30-a9482e743aa&title=&width=346.4)
于是，对应的线性方程组也可以由$\!H\Delta x=g\!$变为如下形式：
$\begin{bmatrix} \bm{B} & \bm{E} \\ \bm{E}^T & \bm{C} \end{bmatrix} \begin{bmatrix} \Delta \bm{x}_c \\ \Delta \bm{x}_p \end{bmatrix} = \begin{bmatrix} \bm{v} \\ \bm{w} \end{bmatrix}.$
其中$\!\bm{B}\!$是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数是相机变量的个数。由于路标数量会远远大于相机变量个数，所以$\!\bm{C}\!$往往也远大于$\!\bm{B}\!$。三维空间中每个路标点为三维，于是$\!\bm{C}\!$矩阵为对角块矩阵，每个块为$\!3\!\times \!3\!$矩阵。对角块矩阵求逆的难度远小于对一般矩阵的求逆难度，因为我们只需要对那些对角线矩阵块分别求逆即可。考虑到这个特性，我们对线性方程组进行高斯消元，目标是消去右上角的非对角部分E,得
$\begin{bmatrix} \bm{I} & -\bm{EC}^{-1} \\ 0 & \bm{I} \end{bmatrix} \begin{bmatrix} \bm{B} & \bm{E} \\ \bm{E}^T & \bm{C} \end{bmatrix} \begin{bmatrix} \Delta\bm{x}_c \\ \Delta\bm{x}_p \end{bmatrix} = \begin{bmatrix} \bm{I} & -\bm{EC}^{-1} \\ 0 & \bm{I} \end{bmatrix} \begin{bmatrix} \bm{v} \\ \bm{w} \end{bmatrix}.$
整理，得
$\begin{bmatrix} \bm{B}-\bm{EC}^{-1}\bm{E}^T & 0 \\ \bm{E}^T & \bm{C} \end{bmatrix} \begin{bmatrix} \Delta\bm{x}_c \\ \Delta\bm{x}_p \end{bmatrix} = \begin{bmatrix} \bm{v}-\bm{EC}^{-1}\bm{w} \\ \bm{w} \end{bmatrix}.$
消元之后，方程组第一行变成和$\Delta\bm{x}$无关的项。单独把它拿出来，得到关于位姿部分的增量方程：
$[\bm{B}-\bm{EC}^{-1}\bm{E}^T]\Delta \bm{x}_c=\bm{v}-\bm{EC}^{-1}\bm{w}.$
这个线性方程的维度和$\!\!B\!$矩阵一样。我们的做法是先求解这个方程，然后把解得的$\!\!\Delta\bm{x}\!\!$代入原方程，求解$\!\!\Delta\bm{x}_p\!$。这个过程称为Marginalization，或者Schur消元（Schur Elimination）。相比于直接解线性方程的做法，它的优势在于：
1.在消元过程中，由于$\!\bm{C}\!$为对角块，所以$\!\bm{C}^{-1}\!$容易解出。
2.求解了$\!\!\Delta\bm{x}_c\!$之后，路标部分的增量方程由$\!\!\Delta\bm{x}_p\!=\!\bm{C}^{-1}(\bm{w}-\bm{E}^T\Delta\bm{x}_c)\!$给出。这依然用到了$\!\bm{C}^{-1}\!$易于求解的特性。 最后，在求得$\bm{x}$的增量后，更新变量值，不断迭代直到问题收敛，最后求得最优的相机位姿及3D点坐标。需要注意的是，**BA优化中需要给一个迭代的初始值，一般做法是不直接进行BA，而是先用PnP算法或者对极几何先获得相机位姿及3D点坐标初始值，然后把初始值交给BA做最终的优化调整**。 ## 本章小结 本章介绍了VSLAM后端常用的非线性优化算法，通常情况下，我们会将后端优化问题构建为一个最小二乘问题，然后使用迭代优化的方式求解。求解中常使用LM算法，但在定位中，可能高斯牛顿法用得也比较多。当然，这部分更多的是理论部分，实际很多优化框架中已经把这些算法都实现好了，如g2o与ceres，到时候直接调用就好了。 ## 本章思考 1.LM法与GN法与梯度下降法的区别与联系是什么？
2.查阅资料，矩阵分解如何求解最小二乘问题。 ## 参考 [1.理解牛顿法](https://zhuanlan.zhihu.com/p/37588590)
[2.非线性最小二乘法之Gauss Newton、L-M、Dog-Leg](https://blog.csdn.net/baoxiao7872/article/details/79914083)
[3.常见的几种最优化方法（梯度下降法、牛顿法、拟牛顿法、共轭梯度法等）](https://www.cnblogs.com/shixiangwan/p/7532830.html)
[4.机器学习中牛顿法凸优化的通俗解释](https://zhuanlan.zhihu.com/p/38541058)
[5.非线性优化整理-3.Levenberg-Marquardt法(LM法)](https://blog.csdn.net/boksic/article/details/79177055)
[6.贝叶斯理论在SLAM状态估计中的应用](https://zhuanlan.zhihu.com/p/113012522)
[7.最小二乘问题的四种解法——牛顿法，梯度下降法，高斯牛顿法和列文伯格-马夸特法的区别和联系](https://zhuanlan.zhihu.com/p/113946848)
[8.SLAM后端：BA优化（Bundle Adjustment）](https://blog.csdn.net/LittleEmperor/article/details/105274299) ================================================ FILE: docs/reference_answers_for_each_chapter/reference_answers_for_each_chapter.md ================================================ # **思考题参考答案** ## 1.一幅图像的诞生：相机投影及相机畸变 ### 1.叙述相机内参的物理意义。如果一部相机的分辨率变为原来的两倍而其他地方不变，那么它的内参将如何变化？ $K=\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)$
fx表示焦距f与像素x方向长度的比值，同理，fy是焦距与像素y方向的长度的比值。cx，cy则表示相机光轴与成像平面的交点，通常位于图像的中心，以像素为单位。
![793c2f46f805a1728f9586d98d17003.jpg](https://cdn.nlark.com/yuque/0/2023/jpeg/1782698/1696235422111-706de6b9-d54f-458e-a546-984cfeb7ab52.jpeg#averageHue=%23fcfbfa&clientId=ua9e19486-0368-4&from=paste&height=386&id=u1340b461&originHeight=738&originWidth=1026&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=64228&status=done&style=none&taskId=u85f3ae59-ecf0-4471-8f16-cbf3bbbf4e7&title=&width=537)
焦距不变，分辨率变为原来两倍，则fx，fy，cx，cy都变为原来两倍。 ### 2.调研全局快门(global shutter)相机和卷帘快门(rolling shutter)相机的异同。它们在SLAM中有何优缺点？ 异同点：
全局快门相机：在全局快门模式下，所有像素同时感光并记录图像。图像的所有行都在同一时间采集，因此可以消除由于快门时间差异引起的运动模糊。这种相机适用于捕捉快速运动的场景，并且能够准确地捕捉瞬态事件。然而，全局快门相机通常需要更多的硬件资源和功耗，并且在高帧率和高分辨率下可能会受到限制。
卷帘快门相机：在卷帘快门模式下，图像传感器的每一行像素在不同的时间采集图像。图像从传感器的顶部到底部逐行进行扫描，因此在高速移动或快速变化的场景中可能会出现图像失真和扭曲。这种相机通常具有较低的功耗和较简单的设计，适用于大多数一般应用。
在SLAM中的优缺点：
全局快门相机优缺点：全局快门相机能够准确地捕捉快速移动的场景，避免了卷帘快门相机可能出现的图像失真问题。这对于快速移动的机器人或快速变化的环境中的SLAM任务非常有用。此外，全局快门相机通常具有较低的运动模糊，有助于提高图像的质量和特征提取的精度。但是全局快门相机通常需要更多的硬件资源和功耗。由于所有像素同时感光，因此在高帧率和高分辨率下可能会受到限制。此外，全局快门相机的成本通常较高。
卷帘快门相机缺点：，具有较低的功耗和成本，但卷帘快门相机在高速移动或快速变化的场景中可能会出现图像失真和扭曲。这可能导致在SLAM任务中的姿态估计和特征匹配等方面出现问题。 ## **2. 差异与统一：坐标系变换与外参标定** ### 1.相机外参的作用是什么？ 相机外参一般指x,y,z,roll,pitch,yaw六个参数，其作用是将相机坐标系的观测转换到载体坐标系，即观测的空间同步。 ### 2.一个载体搭载着摄像头，在平面上运动，相机外参【t(x,y,z)与R(roll,pitch,yaw)】哪些量不可以被标定出来？ z值不能被标定出来。
![1696218831027.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1696218836120-92679af8-2fdf-4c8c-8b8c-5c9b7eb049c4.png#averageHue=%23f6f4f2&clientId=u3a979d62-7169-4&from=paste&height=377&id=u26bac4b7&originHeight=605&originWidth=964&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=102085&status=done&style=none&taskId=u3efb0963-09ab-4d75-95dd-fa2c57bec08&title=&width=600)
标定系列一 | 机器人手眼标定的基础理论分析： [https://zhuanlan.zhihu.com/p/93183788](https://zhuanlan.zhihu.com/p/93183788) ## **3.描述状态不简单：三维空间刚体运动** ### 1.验证四元数旋转某个点后，结果是一个虚四元数（实部为零），所以仍然对应到一个三维空间点。 证明一：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1696220711700-b43b993c-943f-4fba-a90e-bcb7f8b47e74.png#averageHue=%23fcfcfc&clientId=u3a979d62-7169-4&from=paste&height=547&id=uc0656be4&originHeight=684&originWidth=665&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=167525&status=done&style=none&taskId=uc0415328-3d84-4698-b3d9-b4a82d8f094&title=&width=532)
证明二：
![ec087da0453000b3e4a821ab102012e.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1696236686286-239dae28-de45-4cf9-8284-38b7a5a053e4.png#averageHue=%23eaedf1&clientId=ua9e19486-0368-4&from=paste&height=384&id=ub7470ec3&originHeight=647&originWidth=1107&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=70807&status=done&style=none&taskId=u2ce7eff4-4384-46a7-b920-9299f030d84&title=&width=657) ### 2.一般解线性方程Ac=b有哪几种做法？你能在Eigen中实现吗？ 大体分两种做法，闭式解和数值解：
闭式解也称解析解，指在方程有唯一解时，通过求A的逆来求解的方法，方程有解的情况判断如下：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1696221448422-6b5eb822-5908-4ad8-a4b0-f83a18d1c9f6.png#averageHue=%23f6f6f6&clientId=u3a979d62-7169-4&from=paste&height=317&id=ue038cd36&originHeight=485&originWidth=757&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=258951&status=done&style=none&taskId=u39cc2b64-354c-4698-993d-b204c3bb685&title=&width=494.60003662109375)
而求矩阵的逆，则有多种方法，如LU分解，LLT分解，QR分解，SVD分解等等
数值解是在方程没有唯一解，或者求唯一解计算量很大时的一种求近似解的做法，通过不断迭代，求得0点附近的一个解，常用的方法有最小二乘法，通过高斯牛顿或者LM等方法，不断迭代，从而得到一个最佳近似解。 使用Eigen求解方程Ax=b
解析解的方法： ```cpp #include #include int main() { // 定义系数矩阵A和右侧向量b Eigen::MatrixXd A(3, 3); Eigen::VectorXd b(3); // 初始化A和b（这里仅作示例，实际应用中需要根据具体问题进行初始化） A << 1, 2, 3, 4, 5, 6, 7, 8, 10; b << 3, 6, 9; // 解线性方程Ax=b Eigen::VectorXd x = A.colPivHouseholderQr().solve(b); // 输出解x std::cout << "Solution x = \n" << x << std::endl; return 0; } ``` ```cpp // 解线性方程Ax=b（全选主元高斯消元法） Eigen::VectorXd x = A.fullPivLu().solve(b); ``` ```cpp // 解线性方程Ax=b（QR分解法） Eigen::VectorXd x = A.householderQr().solve(b); ``` 其他的求解析解的方法： ```bash // Solve Ax = b. Result stored in x. Matlab: x = A \ b. x = A.ldlt().solve(b)); // A sym. p.s.d. #include x = A.llt().solve(b)); // A sym. p.d. #include x = A.lu().solve(b)); // Stable and fast. #include x = A.qr().solve(b)); // No pivoting. #include x = A.svd().solve(b)); // Stable, slowest. #include // .ldlt() -> .matrixL() and .matrixD() // .llt() -> .matrixL() // .lu() -> .matrixL() and .matrixU() // .qr() -> .matrixQ() and .matrixR() // .svd() -> .matrixU(), .singularValues(), and .matrixV() ``` 迭代的方法： ```cpp // 解线性方程Ax=b（共轭梯度法） Eigen::VectorXd x = A.selfadjointView().ldlt().solve(b); ``` ## **4. 也见森林：视觉SLAM简介** ### 1.VSALM中间接法与直接法的主要区别在什么地方，其各自的优势是什么？ VSALM算法可以使用两种不同的方法来估计相机的运动和生成地图：间接法（indirect method）和直接法（direct method）。
间接法是通过将图像的亮度误差（光度误差）建模为优化问题的残差来估计相机运动。它的主要步骤包括特征提取、特征匹配、通过最小化光度误差进行相机运动估计和地图优化。间接法的优势在于它对光度误差进行了建模，可以利用图像的亮度信息，因此对于光照变化较大的场景具有一定的鲁棒性。然而，间接法需要进行特征提取和匹配，这可能会受到图像质量、特征提取的准确性和匹配的稳定性等因素的影响。
直接法是直接使用图像的像素值作为优化问题的残差进行相机运动估计和地图重建。它不需要进行特征提取和匹配，而是使用整个图像信息进行优化。直接法的优势在于它能够利用更多的像素信息，对纹理较弱或没有特征的场景具有较好的性能。此外，直接法对相机姿态的估计更加精确，因为它利用了像素级别的信息。然而，直接法在处理光照变化和图像噪声时可能会更加敏感。 ### 2.SLAM中前端与后端的关系是什么？ 前端和后端之间的关系是一个迭代的过程。前端负责实时地提供初步的姿态估计和地图信息，但由于传感器噪声、运动模型误差等原因，前端的输出结果可能存在误差和漂移。后端通过优化和融合前端输出的结果，对姿态和地图进行全局一致性的优化，从而提高精度和一致性。优化的结果又反馈给前端，可以用于改进前端的特征提取、匹配方法，提高前端的性能和鲁棒性。
因此，前端和后端是SLAM中相互协作的两个组件。前端负责实时的运动估计和地图构建，后端负责优化和融合，提高整体的精度和一致性。通过前后端的迭代过程，SLAM系统可以实现机器人的自主定位和地图构建。 ## **5.以不变应万变：前端-视觉里程计之特征点** ### 1.请说说SIFT或SURF的原理，并对比它们与ORB之间的优劣。 SIFT的原理是在不同尺度和旋转下，通过高斯差分金字塔和尺度空间极值检测来提取关键点。然后，对这些关键点进行方向估计和描述子生成。SIFT算法具有尺度不变性和旋转不变性，对于光照变化和部分遮挡具有较好的鲁棒性。然而，由于SIFT使用了大量的高斯滤波和高维描述子，计算复杂度较高。
SURF算法是基于Hessian矩阵的特征提取算法。它通过快速近似的方法计算Hessian矩阵的行列式，检测图像中的兴趣点。然后，SURF使用盒子滤波器计算兴趣点的描述子。SURF算法相对于SIFT算法来说更加快速，但在一些情况下可能对旋转和尺度变化的鲁棒性稍差。
相比之下，ORB算法是一种结合了FAST（Features from Accelerated Segment Test）角点检测和BRIEF（Binary Robust Independent Elementary Features）描述子的算法。ORB算法在速度和性能之间取得了平衡。它使用FAST角点检测器快速检测关键点，然后通过BRIEF描述子生成128位的二进制特征描述子。ORB算法具有较快的计算速度和较小的内存需求，适用于实时应用和计算资源有限的设备。然而，相对于SIFT和SURF，ORB算法对光照变化和尺度变化的鲁棒性稍差。
总的来说，SIFT和SURF算法在精度和鲁棒性方面表现较好，但计算复杂度较高。而ORB算法则更快速，适用于实时应用和计算资源有限的环境。选择哪种算法取决于具体应用的需求，如计算资源限制、实时性要求和对鲁棒性的需求。 ### 2.我们发现，OpenCV提供的ORB特征点在图像中分布不够均匀。你是否能够找到或提出让特征点分布更均匀的方法？ 这里有几种方法可以借鉴：
1.栅格法
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1696226461435-84538f41-f758-458a-82af-7044245b5244.png#averageHue=%23c5c4c4&clientId=u67f95e01-f03f-4&from=paste&height=311&id=udebbb932&originHeight=505&originWidth=800&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=198355&status=done&style=none&taskId=u162a7f13-af2a-4dd6-8553-9e89546f1d8&title=&width=493)
在图像上划分出N个网格，将需要提取的总的特征点数，平均分配到每个网格中提取。
2.蒙版法
先使用默认参数进行第一次特征点提取，之后以提取到的特征点坐标为圆心，一定半径生成蒙版，蒙版内只保留一个响应度最大的特征点。剔除距离较近的特征点之后，在蒙版之外的区域，调节ORB提取参数再次进行特征点提取，然后再生成蒙版剔除距离过近的特征点。以上步骤迭代进行，直到特征点数量达到设定值。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1696227241078-08bbff56-3737-4f15-ada7-3798ebad7832.png#averageHue=%23799ad2&clientId=u67f95e01-f03f-4&from=paste&height=345&id=u949fd7a7&originHeight=431&originWidth=561&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=21007&status=done&style=none&taskId=uee45067b-adec-45b2-a7c2-7ac1a9c5568&title=&width=448.8) ## **6.换一个视角看世界：前端-视觉里程计之对极几何** ### 1.本质矩阵的自由度为多少？ ![1696227847586.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1696227853763-cc268fa1-e93e-437f-b0ef-fe43cf23ce6e.png#averageHue=%23eaeaea&clientId=u67f95e01-f03f-4&from=paste&height=207&id=u3b36409c&originHeight=284&originWidth=955&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=168511&status=done&style=none&taskId=udd098e64-4e60-4fbb-b43c-424a7779fe0&title=&width=696) ### 2.直接法求本质矩阵的过程涉及求解齐次线性方程，而对于齐次线性方程的解，要么只有零解，要么有无穷多个解，这里取哪一个解呢？ 由于观测误差的存在，在通过直接线性变换法求本质矩阵时，其方程不是严格等于0的，这个时候一般是通过SVD或者QR分解求其最小二乘法的解。 ## **7.积硅步以至千里：前端-视觉里程计之相对位姿估计** ### 1.EPNP算法为什么会更高效？ 直接线性变换（DLT）求解：EPnP算法采用了直接线性变换（DLT）的思想，通过一个线性的求解步骤来估计相机位姿，而不是使用迭代优化的方法。这种直接求解的方式避免了迭代过程，减少了计算量。
控制点选择：EPnP算法通过选择适当的控制点来提高计算效率。它使用了一个稳健的控制点选择策略，选取那些能够提供更好约束的3D点来估计相机的位姿，而忽略那些提供较弱约束的点。通过减少控制点的数量，EPnP算法可以更快速地计算出相机位姿。
预计算：EPnP算法在求解之前进行了一些预计算步骤，包括将3D点坐标转换到相机坐标系、计算投影矩阵的伪逆等。这些预计算可以在算法执行过程中重复使用，避免了重复计算，提高了计算效率。 ### 2.在特征点匹配过程中，不可避免地会遇到误匹配的情况。如果我们把错误匹配输入到PNP或ICP中，会发生怎样的情况？你能想到哪些避免误匹配的方法？ 如果将错误匹配输入到PNP（Perspective-n-Point）或ICP（Iterative Closest Point）等算法中，可能会导致错误的位姿估计或点云配准结果。
为了避免误匹配，可以考虑以下方法：
1. 特征描述子的选择和匹配策略：使用更具鲁棒性的特征描述子，如SIFT、SURF或ORB，可以减少错误匹配的概率。
2. 外点剔除（Outlier Rejection）策略：通过双向追踪，比值测试，几何一致性验证等方法，可以剔除部分错误匹配的外点。
3.鲁棒性估计的加入：通过采用RANSAC（Random Sample Consensus），可以在存在一定数量的错误匹配时，仍能有效地估计出准确的模型。
4. 多视角信息的利用：通过多视角的信息，例如多个相机或时间序列的图像，可以进行多视角一致性验证，进一步降低错误匹配的概率。
5. 迭代优化的约束：在PNP或ICP等算法中，可以通过引入更多约束条件，如平滑性约束、运动一致性约束等，或者鲁棒核函数的加入来减小错误匹配的影响，提高位姿估计或点云配准的鲁棒性。 ## **8.在不确定中寻找确定：后端之卡尔曼滤波** ### 1.卡尔曼滤波适用于什么系统？ 高斯线性系统，具体说来如下：
1.线性动态系统：卡尔曼滤波假设系统的状态转移方程和观测方程是线性的。
2.高斯噪声假设：卡尔曼滤波假设系统的噪声满足高斯分布。这包括状态转移过程中的过程噪声和观测过程中的观测噪声。高斯噪声假设使得卡尔曼滤波能够通过基于最小均方误差准则进行最优估计。
3.初始条件已知：卡尔曼滤波要求系统的初始状态和误差协方差矩阵是已知的。初始条件提供了系统的先验信息，对估计过程起到重要作用。 ### 2.卡尔曼增益的含义是什么？ 其实质是观测的权重，具体说来如下：
卡尔曼增益的含义是衡量观测值对状态估计的贡献程度，即观测值对于更新系统状态估计的权重。它是根据系统的先验估计和观测模型的可信度来计算的。
卡尔曼增益的计算基于贝叶斯滤波的思想，通过最小化估计值与观测值之间的均方误差，将先验估计和观测值进行加权融合。卡尔曼增益的计算涉及系统的状态协方差矩阵、观测模型的协方差矩阵以及观测噪声的协方差矩阵。
卡尔曼增益的计算公式为：
K = P * H^T * (H * P * H^T + R)^(-1)
其中，K为卡尔曼增益，P为先验估计的状态协方差矩阵，H为观测模型矩阵，R为观测噪声的协方差矩阵。
卡尔曼增益的值越大，观测值在更新系统状态估计中的权重越大。当观测模型和观测噪声越准确时，卡尔曼增益会增大，系统的状态估计会更多地依赖于观测值；而当观测模型或观测噪声不可靠时，卡尔曼增益会减小，系统的状态估计会更多地依赖于先验估计。
卡尔曼增益的作用是实现信息融合，根据系统的先验估计和观测值的相对可靠性，动态调整它们在状态估计中的权重，以得到更准确的系统状态估计。它是卡尔曼滤波算法的关键部分，确保了滤波器的最优性和鲁棒性。 ## **9.每次更好，就是最好：后端之非线性优化** ### 1.LM法与GN法与梯度下降法的区别与联系是什么？ LM法的增量方程为：
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1696232605047-a4ba612f-a38d-4555-ae2a-53c3623d6a25.png#averageHue=%23fbfaf8&clientId=ud9f85d73-210a-4&from=paste&height=40&id=ub65a0936&originHeight=50&originWidth=234&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=3270&status=done&style=none&taskId=u0b640de8-7c57-4274-af21-68d341bee71&title=&width=187.2)
如果$\lambda$很小，甚至接近于0，那么LM就变为GN法，如果$\lambda$值很大，则LM就接近于梯度下降法。 ### 2.查阅资料，矩阵分解如何求解最小二乘问题。 矩阵分解在求解最小二乘问题时，最常用的方法是奇异值分解（Singular Value Decomposition，SVD）和QR分解。
1. 奇异值分解（SVD）：
奇异值分解将一个矩阵分解为三个矩阵的乘积：A = UΣV^T，其中A是待分解的矩阵，U和V是正交矩阵，Σ是一个对角矩阵。奇异值分解可以通过以下步骤求解最小二乘问题：
a. 对矩阵A进行奇异值分解，得到U、Σ和V。
b. 将Σ矩阵中的非零奇异值取倒数，得到Σ^+。
c. 利用矩阵乘法，计算最小二乘解x = VΣ^+U^Tb。
2. QR分解：
QR分解将一个矩阵分解为一个正交矩阵Q和一个上三角矩阵R的乘积：A = QR。QR分解可以通过以下步骤求解最小二乘问题：
a. 对矩阵A进行QR分解，得到正交矩阵Q和上三角矩阵R。
b. 将待求解的向量b进行正交变换，得到新的向量Q^Tb。
c. 利用回代法，求解上三角线性方程组Rx = Q^Tb，得到最小二乘解x。
这些矩阵分解方法可以用于求解超定线性方程组，即方程个数大于未知数个数的情况。最小二乘问题的目标是找到一个最接近于原始方程组的解，使得误差最小化。通过矩阵分解，可以将最小二乘问题转化为求解简化形式的线性方程组，从而得到最小二乘解。
需要注意的是，矩阵分解方法的求解过程中可能涉及到数值稳定性和计算复杂性的问题。在实际应用中，可以根据具体问题和数据特点选择合适的矩阵分解方法，并结合数值计算技巧来提高求解的效率和稳定性。 ## **10.又回到曾经的地点：回环检测** 验证回环检测算法，需要有人工标记回环的数据集。然而人工标记回环是很不方便的，我们会考虑根据标准轨迹计算回环。即，如果轨迹中有两个帧的位姿非常相近，就认为它们是回环。请根据TUM数据集给出的标准轨迹，计算出一个数据集中的回环。这些回环的图像真的相似吗？
由于里程计存在累计误差，通过位姿相似度判断回环存在一定的误差和不确定性。即使两个帧的位姿非常相近，它们的图像内容也可能有一定的差异。这些直接使用轨迹接近的图像帧（剔除时间上很相近的帧）一般来说并不是真的回环的，因此其图像是不相似的。回环检测的意义也是把轨迹不接近，不重合的地图，建立起联系。
了验证回环的相似性，可以对回环帧对所对应的图像进行视觉比对。可以比较它们的特征点、特征描述子等信息，或者进行图像匹配和配准操作。通过视觉比对可以更直观地评估回环帧对的相似性程度。 ## **11.终识庐山真面目：建图** ### 1.是否可以只关注梯度显著的区域，进行立体视觉的半稠密重建。 可以，如直接法的DSO就是这么做的。通过关注梯度显著的区域，可以将计算资源和注意力集中在对深度估计和重建最有帮助的区域上，从而提高算法的效率和准确性。这在实际应用中是很常见的优化策略。 ### 2.如何在八叉树地图中进行导航或路径规划。 先转化为平面栅格地图，注意需要把地面的点云去掉，之后就可以使用路径规划算法（如A*，Dijkstra等算法）在栅格地图上搜索从起点到目标点的最优路径。在搜索过程中，通过考虑节点的代价和启发式函数来评估路径的优劣。 ### 3.了解均匀-高斯混合滤波器的原理与实现。 均匀-高斯混合滤波器（Uniform-Gaussian Mixture Filter）是一种常用的滤波器，常用于目标跟踪和估计问题中。它的原理是通过将目标的状态表示为一个均匀分布和一个高斯分布的混合来进行状态估计。
下面是均匀-高斯混合滤波器的原理和实现步骤：
状态表示：将目标的状态表示为一个均匀分布和一个高斯分布的混合。均匀分布表示目标可能出现的位置的均匀分布，而高斯分布则表示目标位置的不确定性。
预测步骤：使用运动模型对目标的状态进行预测。运动模型可以是线性或非线性的，根据目标的运动特性进行选择。预测步骤通过对均匀分布进行平移，对高斯分布进行卷积来更新目标的状态。
测量更新步骤：使用测量模型将实际观测到的数据与预测的目标状态进行比较，以更新目标的状态估计。测量模型可以是线性或非线性的，根据传感器的特性进行选择。测量更新步骤通过将测量数据与均匀分布和高斯分布进行加权融合来更新目标的状态。
权重更新：根据预测和测量更新的结果，对均匀分布和高斯分布的权重进行更新。权重表示各个分布对目标状态估计的贡献程度。
重采样：根据权重对目标状态进行重采样，以保持粒子的多样性并减小估计误差。
通过以上步骤的迭代，均匀-高斯混合滤波器能够逐步更新目标的状态估计，并提供对目标位置和不确定性的估计。
实现均匀-高斯混合滤波器时，可以使用蒙特卡洛方法（Monte Carlo Methods）进行粒子滤波。具体实现时，可以使用一组粒子来表示均匀分布和高斯分布的混合，并根据预测和测量更新的结果对粒子进行更新和重采样。
需要注意的是，均匀-高斯混合滤波器的性能和效果取决于权重的设置和更新方法，以及运动模型和测量模型的准确性。因此，对于特定的问题和应用场景，需要根据实际情况进行参数调整和算法优化。 ## **12.实践是检验真理的唯一标准：设计VSLAM系统** ### 1.如何把回环模块加入到系统中。 最常用的回环算法是基于外观的回环检测，如DBOW库提供的功能，将回环模块（loop closure module）添加到系统中可以通过以下步骤进行：
1. 数据采集：首先，需要采集足够数量的视觉数据。这些数据将用于后续的回环检测和匹配。
2. 特征提取和描述：对采集到的图像序列进行特征提取和描述，生成能够表示场景的特征向量或描述子。常用的特征包括角点、边缘、ORB特征等。
3. 制定回环检测策略：在视觉里程计的过程中，使用回环检测算法来识别已经经过的场景或视觉回环。回环检测算法可以基于特征匹配、相似性度量等方法来寻找当前帧与历史帧之间的匹配关系。但只有图像之间的相似度还不够，一般还需要制定一些配套的回环检测策略以排除误回环，一般常用的方法有几何一致性验证。
4. 回环匹配后的优化：一旦回环被检测到，需要进行回环匹配和优化。匹配阶段会使用回环检测到的特征描述子与历史帧的特征描述子进行匹配，找到对应的场景点或特征点。然后，通过图优化或其他优化方法，对回环进行校正和调整，以提高整体位姿估计的准确性。
5. 更新地图和状态：根据回环匹配和优化的结果，更新系统中的地图和状态信息。这可能包括更新场景点的位置、更新相机位姿、更新路径估计等。
回环检测一般是接在关键帧检测之后，对每一个关键帧进行回环检测，检测结束之后需要根据检测结果判断是否要进行全局优化。因此其位置是前端之后，再次的全局优化之前。
![image.png](https://cdn.nlark.com/yuque/0/2023/png/1782698/1696234245054-2d89cb09-167f-4154-97be-5d2a5119d019.png#averageHue=%23a69681&clientId=ua9e19486-0368-4&from=paste&height=315&id=u70ba8ed0&originHeight=470&originWidth=620&originalType=binary&ratio=1.25&rotation=0&showTitle=false&size=159864&status=done&style=none&taskId=u46b2a4cf-4e30-4899-80c9-a963ed8cb0e&title=&width=415) ### 2.如何保存地图及加载地图。 1.只保存路标点地图，可以将特征点的坐标及描述子保存为文本，再通过读取文本形式加载（VINS）
2.需要保存稠密点云，可以通过pcl转化为pcd文件保存，或者转为八叉树类型的pcd文件保存及加载。
3.保存栅格地图，此时可以直接保存为png图片形式，然后读取图片进行加载。
4.需要保存图像可视化信息，这个时候需要把每一个关键帧图片都保存下来。