├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── assets └── teaser.jpg ├── cmake └── nhevoConfig.cmake.in ├── example ├── .DS_Store ├── CMakeLists.txt ├── config │ └── config.yaml ├── include │ ├── .DS_Store │ └── nhevo │ │ ├── .DS_Store │ │ ├── detectors │ │ └── detector.hh │ │ ├── nhevo.hh │ │ └── trackers │ │ └── tracker.hh ├── package.xml └── src │ ├── main.cpp │ └── nhevo.cc ├── include ├── .DS_Store ├── nhevo │ ├── solver.hh │ ├── solver_s3c2.hh │ ├── solver_s5c4.hh │ └── solver_s7c6.hh └── opengv │ └── math │ └── Sturm.hpp └── src ├── Sturm.cpp ├── solver.cc ├── solver_s3c2.cc ├── solver_s5c4.cc └── solver_s7c6.cc /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(nhevo VERSION 1.0) 3 | 4 | if (NOT CMAKE_BUILD_TYPE) 5 | set(CMAKE_BUILD_TYPE Release) 6 | endif () 7 | message("Build type: " ${CMAKE_BUILD_TYPE}) 8 | 9 | find_program(CCACHE_PROGRAM ccache) 10 | if(CCACHE_PROGRAM) 11 | set(CMAKE_CXX_COMPILER_LAUNCHER "${CCACHE_PROGRAM}") 12 | set(CMAKE_CUDA_COMPILER_LAUNCHER "${CCACHE_PROGRAM}") 13 | endif() 14 | 15 | set(CMAKE_CXX_STANDARD 17) 16 | set(CMAKE_CXX_STANDARD_REQUIRED True) 17 | set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -Wall -Wextra -pthread") 18 | set(CMAKE_CXX_FLAGS_DEBUG "-g") 19 | set(CMAKE_CXX_FLAGS_RELEASE "-O3") 20 | 21 | 22 | # ------------------- 23 | # build 24 | # ------------------- 25 | find_package(Eigen3 REQUIRED) 26 | find_package(dlib REQUIRED) 27 | 28 | set(HEADERS 29 | include/nhevo/solver.hh 30 | include/nhevo/solver_s3c2.hh 31 | include/nhevo/solver_s5c4.hh 32 | include/nhevo/solver_s7c6.hh 33 | ) 34 | 35 | set(SOURCES 36 | src/Sturm.cpp 37 | src/solver.cc 38 | src/solver_s3c2.cc 39 | src/solver_s5c4.cc 40 | src/solver_s7c6.cc 41 | ) 42 | 43 | add_library(solver SHARED 44 | ${HEADERS} 45 | ${SOURCES} 46 | ) 47 | 48 | target_include_directories(solver PUBLIC 49 | ${EIGEN3_INCLUDE_DIRS} 50 | $ 51 | $ 52 | ) 53 | 54 | target_link_libraries(solver PRIVATE 55 | dlib::dlib 56 | ) 57 | 58 | add_library(nhevo::solver ALIAS solver) 59 | 60 | 61 | # ------------------- 62 | # install 63 | # ------------------- 64 | include(GNUInstallDirs) 65 | 66 | install( 67 | TARGETS solver 68 | EXPORT nhevoTargets 69 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 70 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 71 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 72 | INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 73 | ) 74 | 75 | install( 76 | FILES ${HEADERS} 77 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/nhevo 78 | ) 79 | 80 | install( 81 | EXPORT nhevoTargets 82 | FILE nhevoTargets.cmake 83 | NAMESPACE nhevo:: 84 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/nhevo 85 | ) 86 | 87 | 88 | # ------------------- 89 | # configure 90 | # ------------------- 91 | include(CMakePackageConfigHelpers) 92 | 93 | write_basic_package_version_file( 94 | ${CMAKE_CURRENT_BINARY_DIR}/nhevoConfigVersion.cmake 95 | VERSION ${PROJECT_VERSION} 96 | COMPATIBILITY AnyNewerVersion 97 | ) 98 | 99 | configure_package_config_file( 100 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/nhevoConfig.cmake.in 101 | ${CMAKE_CURRENT_BINARY_DIR}/nhevoConfig.cmake 102 | INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/nhevo 103 | ) 104 | 105 | install( 106 | FILES ${CMAKE_CURRENT_BINARY_DIR}/nhevoConfig.cmake 107 | ${CMAKE_CURRENT_BINARY_DIR}/nhevoConfigVersion.cmake 108 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/nhevo 109 | ) 110 | 111 | export( 112 | EXPORT nhevoTargets 113 | FILE ${CMAKE_CURRENT_BINARY_DIR}/cmake/nhevoTargets.cmake 114 | NAMESPACE nhevo:: 115 | ) 116 | 117 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

NHEVO: Event-based Visual Odometry on Non-holonomic Ground Vehicles

2 |

3 | Wanting Xu*    4 | Si’ao Zhang*    5 | Li Cui*    6 | Xin Peng    7 | Laurent Kneip 8 |

9 | 10 |

11 | *equal contribution 12 |

13 | 14 |

15 | Mobile Perception Lab, 16 | ShanghaiTech 17 |

18 | 19 |

20 | International Conference on 3D Vision (3DV) 2024, Davos, CH 21 |

22 | 23 |

24 | Paper | 25 | arXiv | 26 | Video | 27 | BibTeX 28 |

29 | 30 | 31 | 32 | The open-source implementation of "Event-based Visual Odometry on Non-holonomic Ground Vehicles". 33 | 34 |

35 | 36 |

37 | 38 | We consider visual odometry with a forward-facing event camera mounted on an Ackermann steering vehicle for which motion can be locally approximated by an arc of a circle about an Instantaneous Centre of Rotation (ICR). We assume constant rotational velocity during the time interval for which events are considered. 39 | 40 | ## Abstract 41 | 42 | Despite the promise of superior performance under challenging conditions, event-based motion estimation remains a hard problem owing to the difficulty of extracting and tracking stable features from event streams. In order to robustify the estimation, it is generally believed that fusion with other sensors is a requirement. In this work, we demonstrate reliable, purely event-based visual odometry on planar ground vehicles by employing the constrained non-holonomic motion model of Ackermann steering platforms. We extend single feature n-linearities for regular frame-based cameras to the case of quasi time-continuous event-tracks, and achieve a polynomial form via variable degree Taylor expansions. Robust averaging over multiple event tracks is simply achieved via histogram voting. As demonstrated on both simulated and real data, our algorithm achieves accurate and robust estimates of the vehicle’s instantaneous rotational velocity, and thus results that are comparable to the delta rotations obtained by frame-based sensors under normal conditions. We furthermore significantly outperform the more traditional alternatives in challenging illumination scenarios. 43 | 44 | ## Usage 45 | 46 | ### Dependencies 47 | 48 | - dlib 49 | - Eigen3 50 | 51 | ### Build 52 | 53 | ```bash 54 | git clone git@github.com:gowanting/NHEVO.git ./nhevo 55 | cd ./nhevo 56 | 57 | cmake -B build && cmake --build build 58 | 59 | # optional 60 | cmake --install build 61 | ``` 62 | 63 | ### Example 64 | 65 | We provide a minimal ros example under `example`, you can easily modify `example/src/main.cpp` to remove ros dependency. Parameters are defined in `config/config.yaml`. 66 | 67 | You need to attach your own implementations of a feature detector (Point or Verticle Line) and a tracker, see details in `tracker.hh` and `detector.hh`. 68 | 69 | ```bash 70 | cd ./example 71 | 72 | cmake -B build && cmake --build build 73 | 74 | roscore & 75 | rosbag play your_dataset.bag & 76 | 77 | ./build/nhevo 78 | ``` 79 | 80 | ## Citation 81 | 82 | If you find this work useful, please consider citing: 83 | 84 | ``` 85 | @INPROCEEDINGS{xu24, 86 | title = {Event-based Visual Odometry on Non-holonomic Ground Vehicles}, 87 | author = {Xu,~W. and Zhang,~S. and Cui,~L. and Peng,~X. and Kneip,~L.}, 88 | year = {2024}, 89 | booktitle = {In Proceedings of the International Conference on 3D Vision} 90 | } 91 | ``` 92 | 93 | ## Acknowledgments 94 | This work is inspired by the following work: 95 | 96 | *Huang, Kun, Yifu Wang, and Laurent Kneip. "Motion estimation of non-holonomic ground vehicles from a single feature correspondence measured over n views." Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2019.* ([link](https://openaccess.thecvf.com/content_CVPR_2019/papers/Huang_Motion_Estimation_of_Non-Holonomic_Ground_Vehicles_From_a_Single_Feature_CVPR_2019_paper.pdf)). 97 | 98 | 99 | The authors would also like to thank the fund support from the National Natural Science Foundation of China (62250610225) and Natural Science Foundation of Shanghai (22dz1201900, 22ZR1441300). We also want to acknowledge the generous support of and continued fruitful exchange with our project collaborators at Midea Robozone. 100 | -------------------------------------------------------------------------------- /assets/teaser.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gowanting/NHEVO/b2b2084a1a453453fd668b64651c8bd55f7b2d06/assets/teaser.jpg -------------------------------------------------------------------------------- /cmake/nhevoConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include(CMakeFindDependencyMacro) 4 | 5 | find_dependency(Eigen3 REQUIRED) 6 | 7 | include(${CMAKE_CURRENT_LIST_DIR}/nhevoTargets.cmake) 8 | 9 | check_required_components(nhevo) 10 | -------------------------------------------------------------------------------- /example/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gowanting/NHEVO/b2b2084a1a453453fd668b64651c8bd55f7b2d06/example/.DS_Store -------------------------------------------------------------------------------- /example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(nhevo_ros_example VERSION 1.0) 3 | 4 | if (NOT CMAKE_BUILD_TYPE) 5 | set(CMAKE_BUILD_TYPE Release) 6 | endif () 7 | message("Build type: " ${CMAKE_BUILD_TYPE}) 8 | 9 | find_program(CCACHE_PROGRAM ccache) 10 | if(CCACHE_PROGRAM) 11 | set(CMAKE_CXX_COMPILER_LAUNCHER "${CCACHE_PROGRAM}") 12 | set(CMAKE_CUDA_COMPILER_LAUNCHER "${CCACHE_PROGRAM}") 13 | endif() 14 | 15 | set(CMAKE_CXX_STANDARD 17) 16 | set(CMAKE_CXX_STANDARD_REQUIRED True) 17 | set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -Wall -Wextra -pthread") 18 | set(CMAKE_CXX_FLAGS_DEBUG "-g") 19 | set(CMAKE_CXX_FLAGS_RELEASE "-O3") 20 | 21 | 22 | # ------------------- 23 | # build 24 | # ------------------- 25 | find_package(catkin REQUIRED COMPONENTS 26 | prophesee_event_msgs 27 | geometry_msgs 28 | roscpp 29 | ) 30 | find_package(yaml-cpp REQUIRED) 31 | find_package(Eigen3 REQUIRED) 32 | find_package(nhevo REQUIRED) 33 | 34 | include_directories( 35 | include 36 | ${catkin_INCLUDE_DIRS} 37 | ${EIGEN3_INCLUDE_DIRS} 38 | ) 39 | 40 | add_executable(nhevo 41 | src/main.cpp 42 | src/nhevo.cc 43 | ) 44 | 45 | target_link_libraries(nhevo 46 | ${catkin_LIBRARIES} 47 | ${YAML_CPP_LIBRARIES} 48 | nhevo::solver 49 | ) 50 | 51 | catkin_package( 52 | INCLUDE_DIRS include 53 | ) 54 | -------------------------------------------------------------------------------- /example/config/config.yaml: -------------------------------------------------------------------------------- 1 | general: 2 | width: 608 3 | height: 353 4 | camera_matrix: [[330 , 0. , 304.77865], 5 | [0. , 330 , 172.8540 ], 6 | [0. , 0. , 1. ]] 7 | 8 | solver: 9 | type: "s7c6" 10 | 11 | tau: 0.037 12 | 13 | fmin_eps: 1.0e-5 14 | fmin_max_iter: 10000 15 | fmin_init_radius: 1.0e-9 16 | 17 | find_root_lb: -1.2 18 | find_root_ub: 1.2 19 | find_root_eps_x: 0.001 20 | find_root_eps_val: 0.001 21 | find_root_max_iter: 500 22 | 23 | -------------------------------------------------------------------------------- /example/include/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gowanting/NHEVO/b2b2084a1a453453fd668b64651c8bd55f7b2d06/example/include/.DS_Store -------------------------------------------------------------------------------- /example/include/nhevo/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gowanting/NHEVO/b2b2084a1a453453fd668b64651c8bd55f7b2d06/example/include/nhevo/.DS_Store -------------------------------------------------------------------------------- /example/include/nhevo/detectors/detector.hh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace nhevo { 4 | 5 | class Detector 6 | { 7 | public: 8 | /** 9 | * @brief Determine if the event is a feature point. 10 | * 11 | * @param[in] et The timestamp of the event, in second. 12 | * @param[in] ex The x-axis coordinate. 13 | * @param[in] ey The y-axis coordinate. 14 | * @param[in] ep The polarity. 15 | * 16 | * @return bool. 17 | */ 18 | virtual bool isCorner(const int ex, 19 | const int ey, 20 | const double et, 21 | const bool ep) = 0; 22 | virtual ~Detector() = default; 23 | }; 24 | 25 | } // namespace nhevo 26 | -------------------------------------------------------------------------------- /example/include/nhevo/nhevo.hh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "nhevo/trackers/tracker.hh" 4 | #include "nhevo/detectors/detector.hh" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace nhevo { 16 | 17 | class NHEVO 18 | { 19 | public: 20 | NHEVO(); 21 | 22 | void calculate(); 23 | 24 | void setEvent(const int ex, 25 | const int ey, 26 | const double et, 27 | const bool ep); 28 | 29 | void setGroundTruth(const double stamp, Eigen::Matrix4d& ground_truth); 30 | 31 | private: 32 | void declParams(); 33 | 34 | private: 35 | int width_; 36 | int height_; 37 | Eigen::Matrix K_; 38 | 39 | /* variables */ 40 | std::atomic scale_; 41 | std::atomic gt_del_t_; 42 | std::atomic curr_time_; 43 | 44 | Eigen::Matrix4d pose_; 45 | Eigen::Matrix4d ground_truth_; 46 | Eigen::Matrix4d rel_ground_truth_; 47 | 48 | /* detector */ 49 | std::unique_ptr detector_; 50 | 51 | /* tracker */ 52 | std::unique_ptr tracker_; 53 | 54 | /* solver */ 55 | std::string solver_type_; 56 | std::unique_ptr solver_; 57 | 58 | double solver_tau_; 59 | 60 | double solver_fmin_eps_; 61 | long solver_fmin_max_iter_; 62 | double solver_fmin_init_radius_; 63 | 64 | double solver_find_root_lb_; 65 | double solver_find_root_ub_; 66 | double solver_find_root_eps_x_; 67 | double solver_find_root_eps_val_; 68 | double solver_find_root_max_iter_; 69 | }; 70 | 71 | } // namespace nhevo 72 | -------------------------------------------------------------------------------- /example/include/nhevo/trackers/tracker.hh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace nhevo { 7 | 8 | class Tracker 9 | { 10 | public: 11 | /** 12 | * @brief Add event into the tracker. 13 | * 14 | * @param[in] et The timestamp of the event, in second. 15 | * @param[in] ex The x-axis coordinate. 16 | * @param[in] ey The y-axis coordinate. 17 | * @param[in] ep The polarity. 18 | */ 19 | virtual void addEvent(const int ex, 20 | const int ey, 21 | const double et, 22 | const bool ep) noexcept = 0; 23 | 24 | /** 25 | * @brief Get feature tracks. 26 | * 27 | * @param[out] xss The features' x-axis coordinates, one `std::vector` for each track. 28 | * @param[out] delta_ts The features' timestamps, start from `0` for each vector. 29 | */ 30 | virtual void getXss(std::vector>& xss, 31 | std::vector>& delta_ts) noexcept = 0; 32 | 33 | virtual ~Tracker() = default; 34 | 35 | protected: 36 | std::mutex mutex_; 37 | }; 38 | 39 | } // namespace nhevo 40 | -------------------------------------------------------------------------------- /example/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nhevo_ros_example 4 | 1.0.0 5 | The nhevo ros example 6 | 7 | stevewalker 8 | 9 | TODO 10 | 11 | catkin 12 | 13 | eigen3 14 | yaml-cpp 15 | 16 | roscpp 17 | geometry_msgs 18 | prophesee_event_msgs 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /example/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "nhevo/nhevo.hh" 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | using PoseStamped = geometry_msgs::PoseStamped; 9 | using EventArray = prophesee_event_msgs::EventArray; 10 | 11 | int 12 | main(int argc, char* argv[]) 13 | { 14 | ros::init(argc, argv, "nhevo"); 15 | ros::NodeHandle node; 16 | 17 | auto nhevo = nhevo::NHEVO(); 18 | 19 | ros::Subscriber events_subscriber_ = 20 | node.subscribe("/prophesee/left/events", 100, [&](const EventArray::ConstPtr& msg){ 21 | for (const auto& ev : msg->events) { 22 | nhevo.setEvent(ev.x, 23 | ev.y, 24 | ev.ts.toSec(), 25 | ev.polarity); 26 | } 27 | }); 28 | 29 | ros::Subscriber ground_truth_subscriber_ = 30 | node.subscribe("/gt/pose", 10, [&](const PoseStamped::ConstPtr& msg){ 31 | Eigen::Matrix4d m; 32 | m.setIdentity(); 33 | 34 | m.block<3, 3>(0, 0) = Eigen::Quaternion(msg->pose.orientation.w, 35 | msg->pose.orientation.x, 36 | msg->pose.orientation.y, 37 | msg->pose.orientation.z) 38 | .toRotationMatrix(); 39 | m(0, 3) = msg->pose.position.x; 40 | m(1, 3) = msg->pose.position.y; 41 | m(2, 3) = msg->pose.position.z; 42 | 43 | nhevo.setGroundTruth(msg->header.stamp.toSec(), m); 44 | }); 45 | 46 | auto thread = std::thread( 47 | [&]() { 48 | while (ros::ok()) { 49 | nhevo.calculate(); 50 | } 51 | }); 52 | 53 | ros::MultiThreadedSpinner spinner(2); 54 | spinner.spin(); 55 | 56 | thread.join(); 57 | } 58 | -------------------------------------------------------------------------------- /example/src/nhevo.cc: -------------------------------------------------------------------------------- 1 | #include "nhevo/nhevo.hh" 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace nhevo { 8 | 9 | NHEVO::NHEVO() 10 | { 11 | declParams(); 12 | 13 | scale_ = 0; 14 | pose_.setIdentity(); 15 | ground_truth_.setIdentity(); 16 | 17 | tracker_ = nullptr; 18 | detector_ = nullptr; 19 | 20 | if (solver_type_ == "s3c2") { 21 | solver_ = std::make_unique(); 22 | } else if (solver_type_ == "s5c4") { 23 | solver_ = std::make_unique(); 24 | } else if (solver_type_ == "s7c6") { 25 | solver_ = std::make_unique(); 26 | } 27 | 28 | solver_->setTau(solver_tau_); 29 | 30 | solver_->setFMinBnd(solver_fmin_eps_, 31 | solver_fmin_max_iter_, 32 | solver_fmin_init_radius_); 33 | 34 | solver_->setFindRoot(solver_find_root_lb_, 35 | solver_find_root_ub_, 36 | solver_find_root_eps_x_, 37 | solver_find_root_eps_val_, 38 | solver_find_root_max_iter_); 39 | } 40 | 41 | void 42 | NHEVO::setEvent(const int ex, 43 | const int ey, 44 | const double et, 45 | const bool ep) 46 | { 47 | curr_time_ = et; 48 | 49 | if (detector_->isCorner(ex, ey, et, ep)) { 50 | tracker_->addEvent(ex, ey, et, ep); 51 | } 52 | } 53 | 54 | void 55 | NHEVO::setGroundTruth(const double stamp, Eigen::Matrix4d& ground_truth) 56 | { 57 | if (ground_truth.isIdentity()) { 58 | return; 59 | } 60 | 61 | curr_time_ = stamp; 62 | 63 | static double last = stamp; 64 | gt_del_t_ = stamp - last; 65 | 66 | rel_ground_truth_ = ground_truth_.inverse() * ground_truth; 67 | scale_ = rel_ground_truth_.block<3, 1>(0, 3).norm(); 68 | 69 | last = stamp; 70 | ground_truth_ = std::move(ground_truth); 71 | } 72 | 73 | void 74 | NHEVO::calculate() 75 | { 76 | if (gt_del_t_ < 1e-2) { 77 | return; 78 | } 79 | 80 | auto xss = std::vector>(); 81 | auto delta_ts = std::vector>(); 82 | 83 | tracker_->getXss(xss, delta_ts); 84 | 85 | static double last = curr_time_; 86 | const double elapse = curr_time_ - last; 87 | last = curr_time_; 88 | 89 | auto pose = solver_->getPose(xss, delta_ts, 1e11, elapse); 90 | 91 | auto&& t = pose.block<3, 1>(0, 3); 92 | const auto norm = t.norm(); 93 | 94 | if (abs(norm) > 1e-5) { 95 | t = t * scale_ / norm / (gt_del_t_ / elapse); 96 | } else { 97 | t(0) = scale_ / (gt_del_t_ / elapse); 98 | t(1) = 0; 99 | } 100 | 101 | pose_ = pose_ * pose; 102 | 103 | auto error = (pose.inverse() * rel_ground_truth_ - Eigen::Matrix4d::Identity()).norm(); 104 | std::cout << "error: " << error << "\n"; 105 | 106 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); 107 | } 108 | 109 | void 110 | NHEVO::declParams() 111 | { 112 | auto node = YAML::LoadFile("./config/config.yaml"); 113 | 114 | width_ = node["general"]["width"].as(); 115 | height_ = node["general"]["height"].as(); 116 | 117 | std::vector> vK; 118 | vK = node["general"]["camera_matrix"].as(); 119 | K_ << vK[0][0], vK[0][1], vK[0][2], vK[1][0], vK[1][1], vK[1][2], 120 | vK[2][0], vK[2][1], vK[2][2]; 121 | 122 | solver_type_ = node["solver"]["type"].as(); 123 | 124 | solver_tau_ = node["solver"]["tau"].as(); 125 | 126 | solver_fmin_eps_ = node["solver"]["fmin_eps"].as(); 127 | solver_fmin_max_iter_ = node["solver"]["fmin_max_iter"].as(); 128 | solver_fmin_init_radius_ = node["solver"]["fmin_init_radius"].as(); 129 | 130 | solver_find_root_lb_ = node["solver"]["find_root_lb"].as(); 131 | solver_find_root_ub_ = node["solver"]["find_root_ub"].as(); 132 | solver_find_root_eps_x_ = node["solver"]["find_root_eps_x"].as(); 133 | solver_find_root_eps_val_ = node["solver"]["find_root_eps_val"].as(); 134 | solver_find_root_max_iter_ = node["solver"]["find_root_max_iter"].as(); 135 | } 136 | 137 | } // namespace nhevo 138 | -------------------------------------------------------------------------------- /include/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gowanting/NHEVO/b2b2084a1a453453fd668b64651c8bd55f7b2d06/include/.DS_Store -------------------------------------------------------------------------------- /include/nhevo/solver.hh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace nhevo { 8 | 9 | class Solver 10 | { 11 | public: 12 | /** 13 | * @brief Computes 4x4 transformation matrix (including rotation, translation). 14 | * 15 | * @param[in] xss A set of horizontal bearing measurements. 16 | * @param[in] delta_ts A set of time intervals represents the time relative to the start time t0. 17 | * @param[in] param A numerical parameter that influences the calculation. 18 | * @param[in] elapse Elapsed time which used in the calculation of motion. 19 | * 20 | * @return A 4x4 transformation matrix (including rotation, translation). 21 | */ 22 | Eigen::Matrix getPose( 23 | const std::vector>& xss, 24 | const std::vector>& delta_ts, 25 | const double param, 26 | const double elapse); 27 | 28 | virtual ~Solver() = default; 29 | 30 | /** 31 | * @brief Sets the value of the 'tau' parameter for the Solver object. 32 | * 33 | * @param[in] tau Fixed time interval used to again fix the scale. 34 | * For details, kindly refer to the paper :) 35 | */ 36 | void setTau(const double tau); 37 | 38 | /** 39 | * @brief Sets the parameters for our bound-constrained minimization process in the Solver object. 40 | * 41 | * @param[in] fmin_eps The epsilon value for the minimization procedure, 42 | * controls the precision and stopping criteria. 43 | * @param[in] fmin_max_iter Maximum number of iterations for the minimization procedure, 44 | * control how long the algorithm runs before termination. 45 | * @param[in] fmin_init_radius The initial radius used in the minimization process, 46 | * controls the initial search space. 47 | */ 48 | void setFMinBnd(const double fmin_eps, 49 | const long fmin_max_iter, 50 | const double fmin_init_radius); 51 | 52 | /** 53 | * @brief Sets the parameters for the sturm root-finding algorithm in the Solver object. 54 | * 55 | * @param[in] find_root_lb The lower bound of the interval within which the root is to be searched. 56 | * @param[in] find_root_ub The upper bound of the interval within which the root is to be searched. 57 | * @param[in] find_root_eps_x Tolerance for determining how close to the actual root the solution needs to be. 58 | * @param[in] find_root_eps_val Tolerance for how close the polynomial's value must be to zero to consider a solution a root. 59 | * @param[in] find_root_max_iter The maximum number of iterations the algorithm will perform to find a root. 60 | */ 61 | void setFindRoot(const double find_root_lb, 62 | const double find_root_ub, 63 | const double find_root_eps_x, 64 | const double find_root_eps_val, 65 | const double find_root_max_iter); 66 | 67 | protected: 68 | virtual std::vector solve(const std::vector& xs, 69 | const std::vector& delta_t, 70 | const double param) = 0; 71 | 72 | virtual Eigen::Matrix coeffA(const double x, 73 | const double delta_t) = 0; 74 | 75 | std::vector detSolver(const std::vector& C_det); 76 | 77 | double multiA(const std::vector>& xss, 78 | const std::vector>& delta_ts, 79 | const double param); 80 | 81 | double tau_; 82 | 83 | private: 84 | Eigen::Matrix formulateMultiAc( 85 | const std::vector>& xss, 86 | const std::vector>& delta_t, 87 | const double zN); 88 | 89 | void histogram(const std::vector& v, 90 | std::vector& sort_v, 91 | size_t& lower_bound, 92 | size_t& upper_bound); 93 | 94 | template 95 | std::vector sortIndexes(const std::vector& v) 96 | { 97 | auto idx = std::vector(v.size()); 98 | std::iota(idx.begin(), idx.end(), 0); 99 | 100 | std::sort(idx.begin(), idx.end(), [&v](const size_t& i1, const size_t& i2) { 101 | return v[i1] < v[i2]; 102 | }); 103 | 104 | return idx; 105 | } 106 | 107 | double fmin_eps_; 108 | long fmin_max_iter_; 109 | double fmin_init_radius_; 110 | 111 | double find_root_lb_; 112 | double find_root_ub_; 113 | double find_root_eps_x_; 114 | double find_root_eps_val_; 115 | size_t find_root_max_iter_; 116 | }; 117 | 118 | } 119 | -------------------------------------------------------------------------------- /include/nhevo/solver_s3c2.hh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace nhevo { 6 | 7 | class SolverS3C2 : public Solver 8 | { 9 | public: 10 | SolverS3C2(); 11 | 12 | ~SolverS3C2(); 13 | 14 | std::vector solve(const std::vector& xs, 15 | const std::vector& delta_t, 16 | const double param) override; 17 | 18 | private: 19 | Eigen::Matrix getCoeffs(const double x, const double delta_t); 20 | 21 | std::vector getDetCoeffs(const Eigen::Matrix& C); 22 | 23 | Eigen::Matrix coeffA(const double x, 24 | const double delta_t) override; 25 | }; 26 | 27 | } 28 | -------------------------------------------------------------------------------- /include/nhevo/solver_s5c4.hh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace nhevo { 6 | 7 | class SolverS5C4 : public Solver 8 | { 9 | public: 10 | SolverS5C4(); 11 | 12 | ~SolverS5C4(); 13 | 14 | std::vector solve(const std::vector& xs, 15 | const std::vector& delta_t, 16 | const double param) override; 17 | 18 | private: 19 | Eigen::Matrix getCoeffs(const double x, const double delta_t); 20 | 21 | std::vector getDetCoeffs(const Eigen::Matrix& C); 22 | 23 | Eigen::Matrix coeffA(const double x, 24 | const double delta_t) override; 25 | }; 26 | 27 | } 28 | -------------------------------------------------------------------------------- /include/nhevo/solver_s7c6.hh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace nhevo { 6 | 7 | class SolverS7C6 : public Solver 8 | { 9 | public: 10 | SolverS7C6(); 11 | 12 | ~SolverS7C6(); 13 | 14 | std::vector solve(const std::vector& xs, 15 | const std::vector& delta_t, 16 | const double param) override; 17 | 18 | private: 19 | Eigen::Matrix getCoeffs(const double x, const double delta_t); 20 | 21 | std::vector getDetCoeffs(const Eigen::Matrix& C); 22 | 23 | Eigen::Matrix coeffA(const double x, 24 | const double delta_t) override; 25 | }; 26 | 27 | } 28 | -------------------------------------------------------------------------------- /include/opengv/math/Sturm.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. * 5 | * * 6 | * Redistribution and use in source and binary forms, with or without * 7 | * modification, are permitted provided that the following conditions * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * specific prior written permission. * 17 | * * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 21 | * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE * 22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * 23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * 24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * 25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | /** 32 | * \file Sturm.hpp 33 | * \brief Class for evaluating Sturm chains on polynomials, and bracketing the 34 | * real roots. 35 | */ 36 | 37 | #ifndef OPENGV_STURM_HPP_ 38 | #define OPENGV_STURM_HPP_ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | /** 48 | * \brief The namespace of this library. 49 | */ 50 | namespace opengv { 51 | /** 52 | * \brief The namespace of the math tools. 53 | */ 54 | namespace math { 55 | 56 | class Bracket { 57 | public: 58 | typedef std::shared_ptr Ptr; 59 | typedef std::shared_ptr ConstPtr; 60 | 61 | Bracket(double lowerBound, double upperBound); 62 | Bracket(double lowerBound, double upperBound, size_t changes, 63 | bool setUpperBoundChanges); 64 | virtual ~Bracket(); 65 | 66 | bool dividable(double eps) const; 67 | void divide(std::list &brackets) const; 68 | double lowerBound() const; 69 | double upperBound() const; 70 | bool lowerBoundChangesComputed() const; 71 | bool upperBoundChangesComputed() const; 72 | void setLowerBoundChanges(size_t changes); 73 | void setUpperBoundChanges(size_t changes); 74 | size_t numberRoots() const; 75 | 76 | private: 77 | double _lowerBound; 78 | double _upperBound; 79 | bool _lowerBoundChangesComputed; 80 | bool _upperBoundChangesComputed; 81 | size_t _lowerBoundChanges; 82 | size_t _upperBoundChanges; 83 | }; 84 | 85 | /** 86 | * Sturm is initialized over polynomials of arbitrary order, and used to compute 87 | * the real roots of the polynomial. 88 | */ 89 | class Sturm { 90 | 91 | public: 92 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 93 | /** A pair of values bracketing a real root */ 94 | typedef std::pair bracket_t; 95 | 96 | /** 97 | * \brief Contructor. 98 | * \param[in] p The polynomial coefficients (poly = p(0,0)*x^n + 99 | * p(0,1)*x^(n-1) ...). 100 | */ 101 | Sturm(const Eigen::MatrixXd &p); 102 | /** 103 | * \brief Contructor. 104 | * \param[in] p The polynomial coefficients (poly = p[0]*x^n + p[1]*x^(n-1) 105 | * ...). 106 | */ 107 | Sturm(const std::vector &p); 108 | /** 109 | * \brief Destructor. 110 | */ 111 | virtual ~Sturm(); 112 | 113 | void findRoots3(std::vector &roots, double lb, double ub, 114 | double eps_x = 0.001, double eps_val = 0.001, size_t max_iter = 100); 115 | 116 | void findRoots2(std::vector &roots, double eps_x = 0.001, 117 | double eps_val = 0.001); 118 | /** 119 | * \brief Finds the roots of the polynomial. 120 | * \return An array with the real roots of the polynomial. 121 | */ 122 | std::vector findRoots(); 123 | /** 124 | * \brief Finds brackets for the real roots of the polynomial. 125 | * \return A list of brackets for the real roots of the polynomial. 126 | */ 127 | void bracketRoots(std::vector &roots, double eps = -1.0); 128 | /** 129 | * \brief Evaluates the Sturm chain at a single bound. 130 | * \param[in] bound The bound. 131 | * \return The number of sign changes on the bound. 132 | */ 133 | size_t evaluateChain(double bound); 134 | /** 135 | * \brief Evaluates the Sturm chain at a single bound. 136 | * \param[in] bound The bound. 137 | * \return The number of sign changes on the bound. 138 | */ 139 | size_t evaluateChain2(double bound); 140 | /** 141 | * \brief Composes an initial bracket for all the roots of the polynomial. 142 | * \return The maximum of the absolute values of the bracket-values (That's 143 | * what the Lagrangian bound is able to find). 144 | */ 145 | double computeLagrangianBound(); 146 | 147 | private: 148 | /** 149 | * \brief Internal function used for composing the Sturm chain 150 | * \param[in] p1 First polynomial. 151 | * \param[in] p2 Second polynomial. 152 | * \param[out] r The negated remainder of the polynomial division p1/p2. 153 | */ 154 | void computeNegatedRemainder(const Eigen::MatrixXd &p1, 155 | const Eigen::MatrixXd &p2, Eigen::MatrixXd &r); 156 | 157 | /** A matrix containing the coefficients of the Sturm-chain of the polynomial 158 | */ 159 | Eigen::MatrixXd _C; 160 | /** The dimension _C, which corresponds to (polynomial order+1) */ 161 | size_t _dimension; 162 | }; 163 | 164 | } // namespace math 165 | } // namespace opengv 166 | 167 | #endif /* OPENGV_STURM_HPP_ */ 168 | -------------------------------------------------------------------------------- /src/Sturm.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Author: Laurent Kneip * 3 | * Contact: kneip.laurent@gmail.com * 4 | * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. * 5 | * * 6 | * Redistribution and use in source and binary forms, with or without * 7 | * modification, are permitted provided that the following conditions * 8 | * are met: * 9 | * * Redistributions of source code must retain the above copyright * 10 | * notice, this list of conditions and the following disclaimer. * 11 | * * Redistributions in binary form must reproduce the above copyright * 12 | * notice, this list of conditions and the following disclaimer in the * 13 | * documentation and/or other materials provided with the distribution. * 14 | * * Neither the name of ANU nor the names of its contributors may be * 15 | * used to endorse or promote products derived from this software without * 16 | * specific prior written permission. * 17 | * * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 21 | * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE * 22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * 23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * 24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * 25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * 26 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 27 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 28 | * SUCH DAMAGE. * 29 | ******************************************************************************/ 30 | 31 | 32 | #include 33 | #include 34 | 35 | 36 | 37 | opengv::math::Bracket::Bracket( double lowerBound, double upperBound ) : 38 | _lowerBound(lowerBound), 39 | _upperBound(upperBound), 40 | _lowerBoundChangesComputed(false), 41 | _upperBoundChangesComputed(false), 42 | _lowerBoundChanges(0), 43 | _upperBoundChanges(0) 44 | {} 45 | 46 | opengv::math::Bracket::Bracket( 47 | double lowerBound, double upperBound, size_t changes, bool setUpperBoundChanges ) : 48 | _lowerBound(lowerBound), 49 | _upperBound(upperBound), 50 | _lowerBoundChangesComputed(false), 51 | _upperBoundChangesComputed(false), 52 | _lowerBoundChanges(0), 53 | _upperBoundChanges(0) 54 | { 55 | if( setUpperBoundChanges ) 56 | { 57 | _upperBoundChanges = changes; 58 | _upperBoundChangesComputed = true; 59 | } 60 | else 61 | { 62 | _lowerBoundChanges = changes; 63 | _lowerBoundChangesComputed = true; 64 | } 65 | } 66 | 67 | opengv::math::Bracket::~Bracket() 68 | {} 69 | 70 | bool 71 | opengv::math::Bracket::dividable( double eps ) const 72 | { 73 | if( numberRoots() == 1 && (_upperBound - _lowerBound ) < eps ) 74 | return false; 75 | if( numberRoots() == 0 ) 76 | return false; 77 | double center = (_upperBound + _lowerBound) / 2.0; 78 | if( center == _upperBound || center == _lowerBound) 79 | return false; 80 | return true; 81 | } 82 | 83 | void 84 | opengv::math::Bracket::divide( std::list & brackets ) const 85 | { 86 | double center = (_upperBound + _lowerBound) / 2.0; 87 | Ptr lowerBracket(new Bracket(_lowerBound,center,_lowerBoundChanges,false)); 88 | Ptr upperBracket(new Bracket(center,_upperBound,_upperBoundChanges,true)); 89 | brackets.push_back(lowerBracket); 90 | brackets.push_back(upperBracket); 91 | } 92 | 93 | double 94 | opengv::math::Bracket::lowerBound() const 95 | { 96 | return _lowerBound; 97 | } 98 | 99 | double 100 | opengv::math::Bracket::upperBound() const 101 | { 102 | return _upperBound; 103 | } 104 | 105 | bool 106 | opengv::math::Bracket::lowerBoundChangesComputed() const 107 | { 108 | return _lowerBoundChangesComputed; 109 | } 110 | 111 | bool 112 | opengv::math::Bracket::upperBoundChangesComputed() const 113 | { 114 | return _upperBoundChangesComputed; 115 | } 116 | 117 | void 118 | opengv::math::Bracket::setLowerBoundChanges( size_t changes ) 119 | { 120 | _lowerBoundChanges = changes; 121 | _lowerBoundChangesComputed = true; 122 | } 123 | 124 | void 125 | opengv::math::Bracket::setUpperBoundChanges( size_t changes ) 126 | { 127 | _upperBoundChanges = changes; 128 | _upperBoundChangesComputed = true; 129 | } 130 | 131 | size_t 132 | opengv::math::Bracket::numberRoots() const 133 | { 134 | if( !_lowerBoundChangesComputed || !_upperBoundChangesComputed ) 135 | { 136 | std::cout << "Error: cannot evaluate number of roots" < & p ) : 165 | _C(Eigen::MatrixXd(p.size(),p.size())) 166 | { 167 | _dimension = (size_t) _C.cols(); 168 | _C = Eigen::MatrixXd(_dimension,_dimension); 169 | _C.setZero(); 170 | 171 | for( size_t i = 0; i < _dimension; i++ ) 172 | _C(0,i) = p[i]; 173 | 174 | for( size_t i = 1; i < _dimension; i++ ) 175 | _C(1,i) = _C(0,i-1) * (_dimension-i); 176 | 177 | for( size_t i = 2; i < _dimension; i++ ) 178 | { 179 | Eigen::MatrixXd p1 = _C.block(i-2,i-2,1,_dimension-(i-2)); 180 | Eigen::MatrixXd p2 = _C.block(i-1,i-1,1,_dimension-(i-1)); 181 | Eigen::MatrixXd r; 182 | computeNegatedRemainder(p1,p2,r); 183 | _C.block(i,i,1,_dimension-i) = r.block(0,2,1,_dimension-i); 184 | } 185 | } 186 | 187 | opengv::math::Sturm::~Sturm() {}; 188 | 189 | void opengv::math::Sturm::findRoots3( std::vector & roots, double lb, double ub, double eps_x, double eps_val, size_t max_iter ) 190 | { 191 | std::list stack; 192 | stack.push_back(Bracket::Ptr(new Bracket(lb,ub))); 193 | stack.back()->setLowerBoundChanges( evaluateChain2(stack.back()->lowerBound()) ); 194 | stack.back()->setUpperBoundChanges( evaluateChain2(stack.back()->upperBound()) ); 195 | 196 | // roots.reserve(stack.back()->numberRoots()); 197 | // roots.reserve(10); 198 | 199 | //some variables for pollishing 200 | Eigen::MatrixXd monomials(_dimension,1); 201 | monomials(_dimension-1,0) = 1.0; 202 | 203 | while( !stack.empty() ) 204 | { 205 | Bracket::Ptr bracket = stack.front(); 206 | stack.pop_front(); 207 | 208 | if( bracket->dividable(eps_x) ) 209 | { 210 | bool divide = true; 211 | 212 | if( bracket->numberRoots() == 1 ) 213 | { 214 | //new part, we try immediately to do the pollishing here 215 | bool converged = false; 216 | 217 | double root = 0.5 * (bracket->lowerBound() + bracket->upperBound()); 218 | for(size_t i = 2; i <= _dimension; i++) 219 | monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*root; 220 | Eigen::MatrixXd matValue = _C.row(0) * monomials; 221 | 222 | double value = matValue(0,0); 223 | 224 | int count = 0; 225 | while( !converged && count < max_iter) 226 | { 227 | Eigen::MatrixXd matDerivative = _C.row(1) * monomials; 228 | double derivative = matDerivative(0,0); 229 | 230 | double newRoot = root - (value/derivative); 231 | 232 | if( newRoot < bracket->lowerBound() || newRoot > bracket->upperBound() ) 233 | break; 234 | 235 | for(size_t i = 2; i <= _dimension; i++) 236 | monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*newRoot; 237 | matValue = _C.row(0) * monomials; 238 | double newValue = matValue(0,0); 239 | 240 | if( fabs(newValue) > fabs(value) ) 241 | break; 242 | 243 | //do update 244 | value = newValue; 245 | root = newRoot; 246 | 247 | //check if converged 248 | if( fabs(value) < eps_val ) 249 | converged = true; 250 | 251 | count++; 252 | } 253 | 254 | if( converged ) 255 | { 256 | divide = false; 257 | roots.push_back(root); 258 | } 259 | } 260 | 261 | if(divide) 262 | { 263 | bracket->divide(stack); 264 | std::list::iterator it = stack.end(); 265 | it--; 266 | size_t changes = evaluateChain2((*it)->lowerBound()); 267 | (*it)->setLowerBoundChanges(changes); 268 | it--; 269 | (*it)->setUpperBoundChanges(changes); 270 | } 271 | } 272 | else 273 | { 274 | if( bracket->numberRoots() > 0 ) 275 | roots.push_back(0.5 * (bracket->lowerBound() + bracket->upperBound())); 276 | } 277 | } 278 | } 279 | 280 | void 281 | opengv::math::Sturm::findRoots2( std::vector & roots, double eps_x, double eps_val ) 282 | { 283 | double absoluteBound = computeLagrangianBound(); 284 | std::list stack; 285 | stack.push_back(Bracket::Ptr(new Bracket(-absoluteBound,absoluteBound))); 286 | stack.back()->setLowerBoundChanges( evaluateChain2(stack.back()->lowerBound()) ); 287 | stack.back()->setUpperBoundChanges( evaluateChain2(stack.back()->upperBound()) ); 288 | roots.reserve(stack.back()->numberRoots()); 289 | 290 | //some variables for pollishing 291 | Eigen::MatrixXd monomials(_dimension,1); 292 | monomials(_dimension-1,0) = 1.0; 293 | 294 | while( !stack.empty() ) 295 | { 296 | Bracket::Ptr bracket = stack.front(); 297 | stack.pop_front(); 298 | 299 | if( bracket->dividable(eps_x) ) 300 | { 301 | bool divide = true; 302 | 303 | if( bracket->numberRoots() == 1 ) 304 | { 305 | //new part, we try immediately to do the pollishing here 306 | bool converged = false; 307 | 308 | double root = 0.5 * (bracket->lowerBound() + bracket->upperBound()); 309 | for(size_t i = 2; i <= _dimension; i++) 310 | monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*root; 311 | Eigen::MatrixXd matValue = _C.row(0) * monomials; 312 | 313 | double value = matValue(0,0); 314 | 315 | while( !converged ) 316 | { 317 | Eigen::MatrixXd matDerivative = _C.row(1) * monomials; 318 | double derivative = matDerivative(0,0); 319 | 320 | double newRoot = root - (value/derivative); 321 | 322 | if( newRoot < bracket->lowerBound() || newRoot > bracket->upperBound() ) 323 | break; 324 | 325 | for(size_t i = 2; i <= _dimension; i++) 326 | monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*newRoot; 327 | matValue = _C.row(0) * monomials; 328 | 329 | double newValue = matValue(0,0); 330 | 331 | if( fabs(newValue) > fabs(value) ) 332 | break; 333 | 334 | //do update 335 | value = newValue; 336 | root = newRoot; 337 | 338 | //check if converged 339 | if( fabs(value) < eps_val ) 340 | converged = true; 341 | } 342 | 343 | if( converged ) 344 | { 345 | divide = false; 346 | roots.push_back(root); 347 | } 348 | } 349 | 350 | if(divide) 351 | { 352 | bracket->divide(stack); 353 | std::list::iterator it = stack.end(); 354 | it--; 355 | size_t changes = evaluateChain2((*it)->lowerBound()); 356 | (*it)->setLowerBoundChanges(changes); 357 | it--; 358 | (*it)->setUpperBoundChanges(changes); 359 | } 360 | } 361 | else 362 | { 363 | if( bracket->numberRoots() > 0 ) 364 | roots.push_back(0.5 * (bracket->lowerBound() + bracket->upperBound())); 365 | } 366 | } 367 | } 368 | 369 | std::vector 370 | opengv::math::Sturm::findRoots() 371 | { 372 | //bracket the roots 373 | std::vector roots; 374 | bracketRoots(roots); 375 | 376 | //pollish 377 | Eigen::MatrixXd monomials(_dimension,1); 378 | monomials(_dimension-1,0) = 1.0; 379 | 380 | for(size_t r = 0; r < roots.size(); r++ ) 381 | { 382 | //Now do Gauss iterations here 383 | //evaluate all monomials at the left bound 384 | for(size_t k = 0; k < 5; k++ ) 385 | { 386 | for(size_t i = 2; i <= _dimension; i++) 387 | monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*roots[r]; 388 | 389 | Eigen::MatrixXd value = _C.row(0) * monomials; 390 | Eigen::MatrixXd derivative = _C.row(1) * monomials; 391 | 392 | //correction 393 | roots[r] = roots[r] - (value(0,0)/derivative(0,0)); 394 | } 395 | } 396 | 397 | return roots; 398 | } 399 | 400 | void 401 | opengv::math::Sturm::bracketRoots( std::vector & roots, double eps ) 402 | { 403 | double absoluteBound = computeLagrangianBound(); 404 | std::list stack; 405 | stack.push_back(Bracket::Ptr(new Bracket(-absoluteBound,absoluteBound))); 406 | stack.back()->setLowerBoundChanges( evaluateChain2(stack.back()->lowerBound()) ); 407 | stack.back()->setUpperBoundChanges( evaluateChain2(stack.back()->upperBound()) ); 408 | 409 | double localEps = eps; 410 | if( eps < 0.0 ) 411 | localEps = absoluteBound / (10.0 * stack.back()->numberRoots()); 412 | roots.reserve(stack.back()->numberRoots()); 413 | 414 | while( !stack.empty() ) 415 | { 416 | Bracket::Ptr bracket = stack.front(); 417 | stack.pop_front(); 418 | 419 | if( bracket->dividable( localEps) ) 420 | { 421 | bracket->divide(stack); 422 | std::list::iterator it = stack.end(); 423 | it--; 424 | size_t changes = evaluateChain2((*it)->lowerBound()); 425 | (*it)->setLowerBoundChanges(changes); 426 | it--; 427 | (*it)->setUpperBoundChanges(changes); 428 | } 429 | else 430 | { 431 | if( bracket->numberRoots() > 0 ) 432 | roots.push_back(0.5 * (bracket->lowerBound() + bracket->upperBound())); 433 | } 434 | } 435 | } 436 | 437 | size_t 438 | opengv::math::Sturm::evaluateChain( double bound ) 439 | { 440 | Eigen::MatrixXd monomials(_dimension,1); 441 | monomials(_dimension-1,0) = 1.0; 442 | 443 | //evaluate all monomials at the bound 444 | for(size_t i = 2; i <= _dimension; i++) 445 | monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*bound; 446 | 447 | Eigen::MatrixXd signs(_dimension,1); 448 | for( size_t i = 0; i < _dimension; i++ ) 449 | signs.block(i,0,1,1) = _C.block(i,i,1,_dimension-i) * monomials.block(i,0,_dimension-i,1); 450 | 451 | bool positive = false; 452 | if( signs(0,0) > 0.0 ) 453 | positive = true; 454 | 455 | int signChanges = 0; 456 | 457 | for( size_t i = 1; i < _dimension; i++ ) 458 | { 459 | if( positive ) 460 | { 461 | if( signs(i,0) < 0.0 ) 462 | { 463 | signChanges++; 464 | positive = false; 465 | } 466 | } 467 | else 468 | { 469 | if( signs(i,0) > 0.0 ) 470 | { 471 | signChanges++; 472 | positive = true; 473 | } 474 | } 475 | } 476 | 477 | return signChanges; 478 | } 479 | 480 | size_t 481 | opengv::math::Sturm::evaluateChain2( double bound ) 482 | { 483 | std::vector monomials; 484 | monomials.resize(_dimension); 485 | monomials[_dimension-1] = 1.0; 486 | 487 | //evaluate all monomials at the bound 488 | for(size_t i = 2; i <= _dimension; i++) 489 | monomials[_dimension-i] = monomials[_dimension-i+1]*bound; 490 | 491 | std::vector signs; 492 | signs.reserve(_dimension); 493 | for( size_t i = 0; i < _dimension; i++ ) 494 | { 495 | signs.push_back(0.0); 496 | for( size_t j = i; j < _dimension; j++ ) 497 | signs.back() += _C(i,j) * monomials[j]; 498 | } 499 | 500 | bool positive = false; 501 | if( signs[0] > 0.0 ) 502 | positive = true; 503 | 504 | int signChanges = 0; 505 | 506 | for( size_t i = 1; i < _dimension; i++ ) 507 | { 508 | if( positive ) 509 | { 510 | if( signs[i] < 0.0 ) 511 | { 512 | signChanges++; 513 | positive = false; 514 | } 515 | } 516 | else 517 | { 518 | if( signs[i] > 0.0 ) 519 | { 520 | signChanges++; 521 | positive = true; 522 | } 523 | } 524 | } 525 | 526 | return signChanges; 527 | } 528 | 529 | void 530 | opengv::math::Sturm::computeNegatedRemainder( 531 | const Eigen::MatrixXd & p1, 532 | const Eigen::MatrixXd & p2, 533 | Eigen::MatrixXd & r ) 534 | { 535 | //we have to create 3 subtraction polynomials 536 | Eigen::MatrixXd poly_1(1,p1.cols()); 537 | poly_1.block(0,0,1,p2.cols()) = (p1(0,0)/p2(0,0)) * p2; 538 | poly_1(0,p1.cols()-1) = 0.0; 539 | 540 | Eigen::MatrixXd poly_2(1,p1.cols()); 541 | poly_2.block(0,1,1,p2.cols()) = (p1(0,1)/p2(0,0)) * p2; 542 | poly_2(0,0) = 0.0; 543 | 544 | Eigen::MatrixXd poly_3(1,p1.cols()); 545 | poly_3.block(0,1,1,p2.cols()) = (-p2(0,1)*p1(0,0)/pow(p2(0,0),2)) * p2; 546 | poly_3(0,0) = 0.0; 547 | 548 | //compute remainder 549 | r = -p1 + poly_1 + poly_2 + poly_3; 550 | } 551 | 552 | double 553 | opengv::math::Sturm::computeLagrangianBound() 554 | { 555 | std::vector coefficients; 556 | coefficients.reserve(_dimension-1); 557 | 558 | for(size_t i=0; i < _dimension-1; i++) 559 | coefficients.push_back(pow(fabs(_C(0,i+1)/_C(0,0)),(1.0/(i+1)))); 560 | 561 | size_t j = 0; 562 | double max1 = -1.0; 563 | for( size_t i = 0; i < coefficients.size(); i++ ) 564 | { 565 | if( coefficients[i] > max1 ) 566 | { 567 | j = i; 568 | max1 = coefficients[i]; 569 | } 570 | } 571 | 572 | coefficients[j] = -1.0; 573 | 574 | double max2 = -1.0; 575 | for( size_t i=0; i < coefficients.size(); i++ ) 576 | { 577 | if( coefficients[i] > max2 ) 578 | max2 = coefficients[i]; 579 | } 580 | 581 | double bound = max1 + max2; 582 | return bound; 583 | } 584 | -------------------------------------------------------------------------------- /src/solver.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | namespace nhevo { 7 | 8 | Eigen::Matrix 9 | Solver::getPose(const std::vector>& xss, 10 | const std::vector>& delta_ts, 11 | const double param, 12 | const double elapse) 13 | { 14 | auto pose = Eigen::Matrix(); 15 | pose.setIdentity(); 16 | 17 | const auto omega = multiA(xss, delta_ts, param); 18 | const auto theta = -omega * elapse; 19 | const auto C = cos(theta); 20 | const auto S = sin(theta); 21 | 22 | pose(0, 0) = pose(1, 1) = C; 23 | pose(0, 1) = -S; 24 | pose(1, 0) = S; 25 | 26 | if (theta > 0) { 27 | pose(0, 3) = S; 28 | pose(1, 3) = 1 - C; 29 | } else { 30 | pose(0, 3) = -S; 31 | pose(1, 3) = C - 1; 32 | } 33 | 34 | return pose; 35 | } 36 | 37 | double 38 | Solver::multiA(const std::vector>& xss, 39 | const std::vector>& delta_ts, 40 | const double param) 41 | { 42 | if (xss.empty()) { 43 | return 0; 44 | } 45 | 46 | auto sols = std::vector(); 47 | auto map = std::vector(); 48 | 49 | for (auto k = 0; k < xss.size(); k++) { 50 | const auto& xs = xss[k]; 51 | const auto& delta_t = delta_ts[k]; 52 | auto z = solve(xs, delta_t, param); 53 | 54 | if (!z.empty()) { 55 | for (auto i = 0; i < z.size(); i++) { 56 | map.push_back(k); 57 | } 58 | sols.insert(sols.end(), z.begin(), z.end()); 59 | } 60 | } 61 | if (sols.empty()) { 62 | std::cerr << "sols is empty \n"; 63 | return 0; 64 | } 65 | 66 | auto sort_v = std::vector(); 67 | size_t lower_bound; 68 | size_t upper_bound; 69 | histogram(sols, sort_v, lower_bound, upper_bound); 70 | 71 | double start_point = 72 | sols[sort_v[static_cast((lower_bound + upper_bound) / 2)]]; 73 | const double begin = sols[sort_v[lower_bound]]; 74 | const double end = sols[sort_v[upper_bound]]; 75 | 76 | auto minSingular = [&xss, &delta_ts, this](const double zN) { 77 | const auto A = formulateMultiAc(xss, delta_ts, zN); 78 | Eigen::BDCSVD svd(A); 79 | auto s = svd.singularValues(); 80 | return s.minCoeff(); 81 | }; 82 | 83 | dlib::find_min_single_variable(minSingular, 84 | start_point, 85 | begin, 86 | end, 87 | fmin_eps_, 88 | fmin_max_iter_, 89 | fmin_init_radius_); 90 | return start_point; 91 | } 92 | 93 | std::vector 94 | Solver::detSolver(const std::vector& C_det) 95 | { 96 | std::vector dCoeffs, d2Coeffs, roots, sol; 97 | 98 | dCoeffs.resize(C_det.size() - 1); 99 | for (size_t i = 1; i < C_det.size(); i++) { 100 | dCoeffs[i - 1] = C_det[i] * i; 101 | } 102 | 103 | d2Coeffs.resize(C_det.size() - 2); 104 | for (size_t i = 1; i < dCoeffs.size(); i++) { 105 | d2Coeffs[i - 1] = dCoeffs[i] * i; 106 | } 107 | 108 | std::reverse(dCoeffs.begin(), dCoeffs.end()); 109 | opengv::math::Sturm sturm(dCoeffs); 110 | sturm.findRoots3(roots, 111 | find_root_lb_, 112 | find_root_ub_, 113 | find_root_eps_x_, 114 | find_root_eps_val_, 115 | find_root_max_iter_); 116 | // sturm.findRoots2(roots, 0.0045, 1e-8); 117 | 118 | for (auto z : roots) { 119 | double value = d2Coeffs[0]; 120 | for (size_t i = 1; i < d2Coeffs.size(); i++) { 121 | value += d2Coeffs[i] * pow(z, i); 122 | } 123 | 124 | if (value > 0) 125 | sol.push_back(z); 126 | } 127 | return sol; 128 | } 129 | 130 | Eigen::Matrix 131 | Solver::formulateMultiAc(const std::vector>& xss, 132 | const std::vector>& delta_t, 133 | const double zN) 134 | { 135 | const auto inlier_num = xss.size(); 136 | auto rows = 0; 137 | for (size_t i = 0; i < inlier_num; i++) { 138 | rows = rows + xss[i].size(); 139 | } 140 | auto cols = 2 * inlier_num + 1; 141 | 142 | auto multi_ac = Eigen::Matrix(); 143 | multi_ac.resize(rows, cols); 144 | multi_ac.setZero(); 145 | 146 | auto count = 0; 147 | for (size_t i = 0; i < inlier_num; i++) { 148 | for (size_t j = 0; j < xss[i].size(); j++) { 149 | auto cur_C = coeffA(xss[i][j], delta_t[i][j]); 150 | 151 | auto c = Eigen::Matrix(); 152 | c.setZero(); 153 | for (size_t kk = 0; kk < 3; kk++) { 154 | for (size_t k = 0; k < cur_C.cols(); k++) { 155 | c(kk) = c(kk) + cur_C(kk, k) * std::pow(zN, k - 1); 156 | } 157 | } 158 | 159 | multi_ac(count, 2 * i) = c(0); 160 | multi_ac(count, 2 * i + 1) = c(1); 161 | multi_ac(count, multi_ac.cols() - 1) = c(2); 162 | 163 | count++; 164 | } 165 | } 166 | 167 | multi_ac = multi_ac / rows; 168 | return multi_ac; 169 | } 170 | 171 | void 172 | Solver::histogram(const std::vector& v, 173 | std::vector& sort_v, 174 | size_t& lower_bound, 175 | size_t& upper_bound) 176 | { 177 | lower_bound = 0; 178 | upper_bound = 0; 179 | sort_v = sortIndexes(v); 180 | 181 | const auto size = v.size(); 182 | if (size <= 1) { 183 | return; 184 | } 185 | 186 | const auto IQR = v[sort_v[size * 3 / 4]] - v[sort_v[size / 4]]; 187 | 188 | const auto min = v[sort_v[0]]; 189 | const auto max = v[sort_v[size - 1]]; 190 | const auto bin_width = 2 * IQR / (std::cbrt(static_cast(size))); 191 | const int bin_num = std::ceil((max - min) / bin_width); 192 | 193 | if (!IQR || bin_num <= 0) { 194 | return; 195 | } 196 | 197 | auto N = std::vector(); 198 | auto values = std::vector>( 199 | bin_num, std::array{ 0, size, size }); 200 | for (size_t i = 0; i < size; i++) { 201 | const size_t index = std::floor((v[sort_v[i]] - min) / bin_width); 202 | values[index][0]++; 203 | 204 | auto& min_idx = values[index][1]; 205 | auto& max_idx = values[index][2]; 206 | if (min_idx == size) { 207 | min_idx = i; 208 | } 209 | max_idx = i; 210 | } 211 | for (const auto& it : values) { 212 | N.push_back(it[0]); 213 | } 214 | 215 | const auto sort_N = sortIndexes(N); 216 | const auto& _1 = sort_N[bin_num - 1]; 217 | const auto& _2 = sort_N[bin_num - 2]; 218 | 219 | if (std::abs(static_cast(_1) - static_cast(_2)) == 1 && 220 | 1.1 * N[_2] > N[_1]) { 221 | if (_1 < _2) { 222 | lower_bound = values[_1][1]; 223 | upper_bound = values[_2][2]; 224 | } else { 225 | lower_bound = values[_2][1]; 226 | upper_bound = values[_1][2]; 227 | } 228 | return; 229 | } 230 | 231 | lower_bound = values[_1][1]; 232 | upper_bound = values[_1][2]; 233 | } 234 | 235 | void 236 | Solver::setTau(const double tau) 237 | { 238 | tau_ = tau; 239 | } 240 | 241 | void 242 | Solver::setFMinBnd(const double fmin_eps, 243 | const long fmin_max_iter, 244 | const double fmin_init_radius) 245 | { 246 | fmin_eps_ = fmin_eps; 247 | fmin_max_iter_ = fmin_max_iter; 248 | fmin_init_radius_ = fmin_init_radius; 249 | } 250 | 251 | void 252 | Solver::setFindRoot(const double find_root_lb, 253 | const double find_root_ub, 254 | const double find_root_eps_x, 255 | const double find_root_eps_val, 256 | const double find_root_max_iter) 257 | { 258 | find_root_lb_ = find_root_lb; 259 | find_root_ub_ = find_root_ub; 260 | find_root_eps_x_ = find_root_eps_x; 261 | find_root_eps_val_ = find_root_eps_val; 262 | find_root_max_iter_ = find_root_max_iter; 263 | } 264 | 265 | } 266 | --------------------------------------------------------------------------------