├── CMakeLists.txt
├── LICENSE
├── README.md
├── README_CN.md
├── demo
├── bboxes.yaml
├── config.yaml
└── demo.mp4
├── include
├── argparse
│ └── argparser.h
├── image_utils
│ ├── detect_process.h
│ ├── lap
│ │ ├── _lapjv.pyx
│ │ ├── lapjv.cpp
│ │ ├── lapjv.h
│ │ └── lapmod.cpp
│ └── tracker.h
└── print_utils.h
├── scripts
└── estimate_cam_param.py
└── src
└── main.cpp
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.6)
2 |
3 | project(trt_det LANGUAGES CXX)
4 |
5 | add_definitions(-std=c++11)
6 |
7 | set(CMAKE_CXX_STANDARD 11)
8 | set(CMAKE_BUILD_TYPE Release)
9 |
10 | include_directories(
11 | include
12 | )
13 |
14 | find_package(OpenCV REQUIRED)
15 | include_directories(${OpenCV_INCLUDE_DIRS})
16 |
17 |
18 | find_package(Eigen3 REQUIRED)
19 | include_directories(/usr/include/eigen3)
20 |
21 |
22 | add_executable(ucmc_track
23 | src/main.cpp
24 | include/image_utils/lap/lapjv.cpp
25 | include/image_utils/lap/lapmod.cpp
26 | )
27 |
28 | target_link_libraries(ucmc_track yaml-cpp)
29 | target_link_libraries(ucmc_track ${OpenCV_LIBS})
30 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | Apache License
2 | Version 2.0, January 2004
3 | http://www.apache.org/licenses/
4 |
5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
6 |
7 | 1. Definitions.
8 |
9 | "License" shall mean the terms and conditions for use, reproduction,
10 | and distribution as defined by Sections 1 through 9 of this document.
11 |
12 | "Licensor" shall mean the copyright owner or entity authorized by
13 | the copyright owner that is granting the License.
14 |
15 | "Legal Entity" shall mean the union of the acting entity and all
16 | other entities that control, are controlled by, or are under common
17 | control with that entity. For the purposes of this definition,
18 | "control" means (i) the power, direct or indirect, to cause the
19 | direction or management of such entity, whether by contract or
20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the
21 | outstanding shares, or (iii) beneficial ownership of such entity.
22 |
23 | "You" (or "Your") shall mean an individual or Legal Entity
24 | exercising permissions granted by this License.
25 |
26 | "Source" form shall mean the preferred form for making modifications,
27 | including but not limited to software source code, documentation
28 | source, and configuration files.
29 |
30 | "Object" form shall mean any form resulting from mechanical
31 | transformation or translation of a Source form, including but
32 | not limited to compiled object code, generated documentation,
33 | and conversions to other media types.
34 |
35 | "Work" shall mean the work of authorship, whether in Source or
36 | Object form, made available under the License, as indicated by a
37 | copyright notice that is included in or attached to the work
38 | (an example is provided in the Appendix below).
39 |
40 | "Derivative Works" shall mean any work, whether in Source or Object
41 | form, that is based on (or derived from) the Work and for which the
42 | editorial revisions, annotations, elaborations, or other modifications
43 | represent, as a whole, an original work of authorship. For the purposes
44 | of this License, Derivative Works shall not include works that remain
45 | separable from, or merely link (or bind by name) to the interfaces of,
46 | the Work and Derivative Works thereof.
47 |
48 | "Contribution" shall mean any work of authorship, including
49 | the original version of the Work and any modifications or additions
50 | to that Work or Derivative Works thereof, that is intentionally
51 | submitted to Licensor for inclusion in the Work by the copyright owner
52 | or by an individual or Legal Entity authorized to submit on behalf of
53 | the copyright owner. For the purposes of this definition, "submitted"
54 | means any form of electronic, verbal, or written communication sent
55 | to the Licensor or its representatives, including but not limited to
56 | communication on electronic mailing lists, source code control systems,
57 | and issue tracking systems that are managed by, or on behalf of, the
58 | Licensor for the purpose of discussing and improving the Work, but
59 | excluding communication that is conspicuously marked or otherwise
60 | designated in writing by the copyright owner as "Not a Contribution."
61 |
62 | "Contributor" shall mean Licensor and any individual or Legal Entity
63 | on behalf of whom a Contribution has been received by Licensor and
64 | subsequently incorporated within the Work.
65 |
66 | 2. Grant of Copyright License. Subject to the terms and conditions of
67 | this License, each Contributor hereby grants to You a perpetual,
68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
69 | copyright license to reproduce, prepare Derivative Works of,
70 | publicly display, publicly perform, sublicense, and distribute the
71 | Work and such Derivative Works in Source or Object form.
72 |
73 | 3. Grant of Patent License. Subject to the terms and conditions of
74 | this License, each Contributor hereby grants to You a perpetual,
75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
76 | (except as stated in this section) patent license to make, have made,
77 | use, offer to sell, sell, import, and otherwise transfer the Work,
78 | where such license applies only to those patent claims licensable
79 | by such Contributor that are necessarily infringed by their
80 | Contribution(s) alone or by combination of their Contribution(s)
81 | with the Work to which such Contribution(s) was submitted. If You
82 | institute patent litigation against any entity (including a
83 | cross-claim or counterclaim in a lawsuit) alleging that the Work
84 | or a Contribution incorporated within the Work constitutes direct
85 | or contributory patent infringement, then any patent licenses
86 | granted to You under this License for that Work shall terminate
87 | as of the date such litigation is filed.
88 |
89 | 4. Redistribution. You may reproduce and distribute copies of the
90 | Work or Derivative Works thereof in any medium, with or without
91 | modifications, and in Source or Object form, provided that You
92 | meet the following conditions:
93 |
94 | (a) You must give any other recipients of the Work or
95 | Derivative Works a copy of this License; and
96 |
97 | (b) You must cause any modified files to carry prominent notices
98 | stating that You changed the files; and
99 |
100 | (c) You must retain, in the Source form of any Derivative Works
101 | that You distribute, all copyright, patent, trademark, and
102 | attribution notices from the Source form of the Work,
103 | excluding those notices that do not pertain to any part of
104 | the Derivative Works; and
105 |
106 | (d) If the Work includes a "NOTICE" text file as part of its
107 | distribution, then any Derivative Works that You distribute must
108 | include a readable copy of the attribution notices contained
109 | within such NOTICE file, excluding those notices that do not
110 | pertain to any part of the Derivative Works, in at least one
111 | of the following places: within a NOTICE text file distributed
112 | as part of the Derivative Works; within the Source form or
113 | documentation, if provided along with the Derivative Works; or,
114 | within a display generated by the Derivative Works, if and
115 | wherever such third-party notices normally appear. The contents
116 | of the NOTICE file are for informational purposes only and
117 | do not modify the License. You may add Your own attribution
118 | notices within Derivative Works that You distribute, alongside
119 | or as an addendum to the NOTICE text from the Work, provided
120 | that such additional attribution notices cannot be construed
121 | as modifying the License.
122 |
123 | You may add Your own copyright statement to Your modifications and
124 | may provide additional or different license terms and conditions
125 | for use, reproduction, or distribution of Your modifications, or
126 | for any such Derivative Works as a whole, provided Your use,
127 | reproduction, and distribution of the Work otherwise complies with
128 | the conditions stated in this License.
129 |
130 | 5. Submission of Contributions. Unless You explicitly state otherwise,
131 | any Contribution intentionally submitted for inclusion in the Work
132 | by You to the Licensor shall be under the terms and conditions of
133 | this License, without any additional terms or conditions.
134 | Notwithstanding the above, nothing herein shall supersede or modify
135 | the terms of any separate license agreement you may have executed
136 | with Licensor regarding such Contributions.
137 |
138 | 6. Trademarks. This License does not grant permission to use the trade
139 | names, trademarks, service marks, or product names of the Licensor,
140 | except as required for reasonable and customary use in describing the
141 | origin of the Work and reproducing the content of the NOTICE file.
142 |
143 | 7. Disclaimer of Warranty. Unless required by applicable law or
144 | agreed to in writing, Licensor provides the Work (and each
145 | Contributor provides its Contributions) on an "AS IS" BASIS,
146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147 | implied, including, without limitation, any warranties or conditions
148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149 | PARTICULAR PURPOSE. You are solely responsible for determining the
150 | appropriateness of using or redistributing the Work and assume any
151 | risks associated with Your exercise of permissions under this License.
152 |
153 | 8. Limitation of Liability. In no event and under no legal theory,
154 | whether in tort (including negligence), contract, or otherwise,
155 | unless required by applicable law (such as deliberate and grossly
156 | negligent acts) or agreed to in writing, shall any Contributor be
157 | liable to You for damages, including any direct, indirect, special,
158 | incidental, or consequential damages of any character arising as a
159 | result of this License or out of the use or inability to use the
160 | Work (including but not limited to damages for loss of goodwill,
161 | work stoppage, computer failure or malfunction, or any and all
162 | other commercial damages or losses), even if such Contributor
163 | has been advised of the possibility of such damages.
164 |
165 | 9. Accepting Warranty or Additional Liability. While redistributing
166 | the Work or Derivative Works thereof, You may choose to offer,
167 | and charge a fee for, acceptance of support, warranty, indemnity,
168 | or other liability obligations and/or rights consistent with this
169 | License. However, in accepting such obligations, You may act only
170 | on Your own behalf and on Your sole responsibility, not on behalf
171 | of any other Contributor, and only if You agree to indemnify,
172 | defend, and hold each Contributor harmless for any liability
173 | incurred by, or claims asserted against, such Contributor by reason
174 | of your accepting any such warranty or additional liability.
175 |
176 | END OF TERMS AND CONDITIONS
177 |
178 | APPENDIX: How to apply the Apache License to your work.
179 |
180 | To apply the Apache License to your work, attach the following
181 | boilerplate notice, with the fields enclosed by brackets "[]"
182 | replaced with your own identifying information. (Don't include
183 | the brackets!) The text should be enclosed in the appropriate
184 | comment syntax for the file format. We also recommend that a
185 | file or class name and description of purpose be included on the
186 | same "printed page" as the copyright notice for easier
187 | identification within third-party archives.
188 |
189 | Copyright [yyyy] [name of copyright owner]
190 |
191 | Licensed under the Apache License, Version 2.0 (the "License");
192 | you may not use this file except in compliance with the License.
193 | You may obtain a copy of the License at
194 |
195 | http://www.apache.org/licenses/LICENSE-2.0
196 |
197 | Unless required by applicable law or agreed to in writing, software
198 | distributed under the License is distributed on an "AS IS" BASIS,
199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
200 | See the License for the specific language governing permissions and
201 | limitations under the License.
202 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # UCMCTrack C++ Deployment
2 |
3 |
4 |
5 | [简体中文](README_CN.md)
6 |
7 |
8 |
9 | 
10 |
11 |
12 | ## Before using
13 |
14 | Please read [the paper](https://arxiv.org/abs/2312.08952) first!
15 |
16 | Please read [the paper](https://arxiv.org/abs/2312.08952) first!
17 |
18 | Please read [the paper](https://arxiv.org/abs/2312.08952) first!
19 |
20 | **DO NOT USE THIS PROJECT IF YOU KNOW NOTHING ABOUT THIS ALGORITHM!**
21 |
22 | ## Acknowledgements
23 |
24 | First and foremost, I would like to thank [corfyi](https://github.com/corfyi) for open-sourcing this multi-object tracking algorithm. However, as mentioned in the issues, while the author does have a C++ version of the code, it has not been made public. Despite my limited expertise, I spent four days converting the original project ([UCMCTrack](https://github.com/corfyi/UCMCTrack)) from Python to C++ code. And if you find any bugs or inconsistencies in the algorithm logic, please feel free to raise them in the issues section.
25 |
26 |
27 | ## C++ Is Blazing Fast!
28 |
29 | The measured FPS of the Python version of the code is less than 100Hz. In the original paper, the author mentioned that the C++ version of their code can achieve over 1000+ FPS in pure tracking scenarios. If my implementation is correct, it seems to reach around 3000FPS in the demo.mp4 (using an I5-11320H CPU), as shown in the image below. When combined with YOLO, the speed is even faster (due to the GPU inference latency reducing the CPU load).
30 |
31 | 
32 |
33 | Even on an ARM platform (Orange Pi, RK3588 chip), the speed is almost unchanged (with CPU utilization below 10%). However, when combined with a neural network, there is a significant slowdown (as RKNN not only loads the NPU but also the CPU). With an average CPU utilization of 45%, the latency is approximately 8 ms.
34 |
35 | 
36 |
37 |
38 | ## Dependencies
39 |
40 | - libopencv-dev
41 | - libeigen3-dev
42 | - libyaml-cpp0.6
43 |
44 |
45 | ## Notes
46 |
47 | - This project does not provide code for the object detection part. The example code uses the detection results from the demo.mp4 provided by the original author, which are written to the demo/bboxes.yaml file. The parameters are written to the demo/config.yaml file. You can modify the parameters according to your needs and add detection code to replace the results in the file. The detection results in the file are from a model I trained on VisDrone-DET-2019, see my other project [edgeyolo](https://github.com/LSH9832/edgeyolo). The detection framework also uses the cpp source code provided in that project.
48 |
49 | - The code in include/image_utils/lap is from [gatagat/lap](https://github.com/gatagat/lap), which is the source code for the lap library in Python.
50 |
51 |
52 | ## Setup
53 | Modify the CMakeLists.txt file according to your needs, then
54 | ```shell
55 | mkdir build && cd build
56 | cmake ..
57 | make
58 | ```
59 | ## Using the Demo
60 | Navigate to the build folder and then
61 | ```shell
62 | ./ucmc_track ../demo/bboxes.yaml -f ../demo/config.yaml -p # The '-p' flag pauses the video, press the spacebar to switch to autoplay, or any other key to play frame by frame
63 |
64 | # For detailed usage instructions
65 | ./ucmc_track -?
66 | ```
67 |
68 | ## Adjusting Extrinsic Parameters
69 | ```
70 | python scripts/estimate_cam_param.py --img /path/to/your/img.jpg \ # Use an image file
71 | --vid /path/to/your/video.mp4 \ # Or use a video file, do not use the above option if this one is selected
72 | --id number_of_frame_id \ # Use the number_of_frame_id frame from the video file
73 | --cam_para cfg/track.yaml \ # parameter file
74 | --test-only # Test mode only, without modifying the content of the parameter file
75 | ```
76 |
--------------------------------------------------------------------------------
/README_CN.md:
--------------------------------------------------------------------------------
1 | # UCMCTrack的c++部署
2 |
3 | 
4 |
5 | ## 在使用之前
6 |
7 | 用之前请先阅读[原论文](https://arxiv.org/abs/2312.08952)！
8 |
9 | 用之前请先阅读[原论文](https://arxiv.org/abs/2312.08952)！
10 |
11 | 用之前请先阅读[原论文](https://arxiv.org/abs/2312.08952)！
12 |
13 | **不要在完全不了解算法原理的情况下使用本项目，效果不好概不负责，谢谢！**
14 |
15 | ## 致谢
16 |
17 | 首先感谢大佬[corfyi](https://github.com/corfyi)将此多目标跟踪算法开源，但在[issues](https://github.com/corfyi/UCMCTrack/issues/18)中看出大佬是有c++版本的代码的，却并没有开源。本人水平有限，工作之余花费4天时间将原项目（[UCMCTrack](https://github.com/corfyi/UCMCTrack)）由python转为c++代码，难免出错，如发现bug或算法逻辑不对的地方欢迎在issues中提出。
18 |
19 |
20 | ## C++真是太快啦！
21 |
22 | 实测python版本的代码FPS不到100Hz, 作者在原文中提到纯跟踪情况下他代码的C++版FPS能达到1000+FPS，如果我的代码没写错的话，在demo.mp4中貌似可以来到3000FPS左右哦，见下图（CPU型号为I5-11320H）,和YOLO神经网络一起运行的时候速度更快（由于GPU推理延时使CPU负载反而变低了）
23 |
24 | 
25 |
26 | 即使在arm平台（香橙派，RK3588芯片）速度也基本没差多少（CPU利用率10%以下），但和神经网络一起跑的时候速度有明显减慢（RKNN不仅让NPU有负载，CPU也有），CPU平均利用率45%时延迟约8毫秒，
27 |
28 | 
29 |
30 |
31 | ## 依赖库
32 |
33 | - libopencv-dev
34 | - libeigen3-dev
35 | - libyaml-cpp0.6
36 |
37 |
38 | ## 注意
39 |
40 | - 本项目不提供目标检测部分的代码，示例代码将原作者提供的demo.mp4中的检测结果写在demo/bboxes.yaml文件中了，参数写在demo/config.yaml中，可根据自己的需求改参数，并加入检测部分的代码以替换文件中的检测结果。文件中的检测结果使用的是本人在VisDrone-DET-2019上训练的模型，详见我的另一个项目[edgeyolo](https://github.com/LSH9832/edgeyolo)。其中检测结果的框架也是采用该项目提供的cpp源码。
41 |
42 | - include/image_utils/lap中的代码来自[gatagat/lap](https://github.com/gatagat/lap)，也就是python中lap库源码。
43 |
44 |
45 | ## Setup
46 | 根据自己的需求改CMakeLists.txt，然后
47 | ```shell
48 | mkdir build && cd build
49 | cmake ..
50 | make
51 | ```
52 | ## 使用Demo
53 | 进入build文件夹，然后
54 | ```shell
55 | ./ucmc_track ../demo/bboxes.yaml -f ../demo/config.yaml -p # -p 可以使画面暂停，按空格键切换为自动播放，按其他键逐帧播放
56 |
57 | # 看具体用法
58 | ./ucmc_track -?
59 | ```
60 |
61 | ## 调整外参矩阵
62 | ```
63 | python scripts/estimate_cam_param.py --img /path/to/your/img.jpg \ # 使用图像文件
64 | --vid /path/to/your/video.mp4 \ # 或者使用视频文件，上面选项就不要写了
65 | --id number_of_frame_id \ # 使用视频文件的第number_of_frame_id帧
66 | --cam_para cfg/track.yaml \ # 参数文件
67 | --test-only # 仅测试，不修改参数文件内容
68 | ```
69 |
--------------------------------------------------------------------------------
/demo/config.yaml:
--------------------------------------------------------------------------------
1 | Ki: [ 1040., 0., 680., 0.,
2 | 0., 1040., 382., 0.,
3 | 0., 0., 1., 0. ]
4 |
5 | Ko: [ -0.33962705646204017, -0.9403932802759871, -0.01771837833151917, 0,
6 | -0.49984364998094355, 0.1964143950108213, -0.843550657048088, 1,
7 | 0.7967495140209739, -0.2776362077328922, -0.5367572524549995, 33.64,
8 | 0, 0, 0, 1 ]
9 |
10 | a1: 100.0
11 | a2: 100.0
12 | wx: 5.0
13 | wy: 5.0
14 | vmax: 10.0
15 | max_age: 10.0
16 | high_score: 0.5
17 | conf_threshold: 0.01
18 |
--------------------------------------------------------------------------------
/demo/demo.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LSH9832/UCMCTrack-cpp/84f617d1815739bf117bcb098a80ef294ddf0cb6/demo/demo.mp4
--------------------------------------------------------------------------------
/include/argparse/argparser.h:
--------------------------------------------------------------------------------
1 | /*
2 | * copy from https://github.com/0382/util/blob/main/cpp/argparse/argparse.hpp
3 | */
4 | #pragma once
5 | #ifndef JSHL_ARGPARSE_HPP
6 | #define JSHL_ARGPARSE_HPP
7 |
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 | #include
16 | #include
17 | #include
18 | #include
19 | #include
20 |
21 | namespace argsutil
22 | {
23 |
24 | // 尽管使用编译器相关的 ABI 函数可以相对比较优雅的实现这个功能
25 | // 但是不同的编译器在某些类型下可能出现一些奇怪的行为
26 | // 最重要的是 std::string 还是免不了要模板特例化
27 | // 因此，不如在这里限定一些类型，免去不可控制的行为
28 |
29 | // 我们仅支持 bool, int, int64_t, double, std::string
30 | // 想要其他长度的类型，获取值之后自行转换
31 | // 当然，如果你愿意的话，自己定义模板特例化也不是不可以
32 |
33 | template
34 | inline std::string type_string()
35 | {
36 | return "null";
37 | }
38 |
39 | template <>
40 | inline std::string type_string()
41 | {
42 | return "bool";
43 | }
44 |
45 | template <>
46 | inline std::string type_string()
47 | {
48 | return "int";
49 | }
50 |
51 | template <>
52 | inline std::string type_string()
53 | {
54 | return "int64_t";
55 | }
56 |
57 | template <>
58 | inline std::string type_string()
59 | {
60 | return "double";
61 | }
62 |
63 | template <>
64 | inline std::string type_string()
65 | {
66 | return "string";
67 | }
68 |
69 | template
70 | std::string to_string(const T &value)
71 | {
72 | std::ostringstream oss;
73 | oss << value;
74 | return oss.str();
75 | }
76 |
77 | template
78 | T parse_value(const std::string &value)
79 | {
80 | std::istringstream iss(value);
81 | T result;
82 | iss >> result;
83 | return result;
84 | }
85 |
86 | struct short_circuit_option
87 | {
88 | short_circuit_option(std::string sname, std::string lname, std::string help, std::function callback)
89 | : short_name(std::move(sname)), long_name(std::move(lname)), help(std::move(help)),
90 | callback(std::move(callback))
91 | {}
92 | std::string short_name;
93 | std::string long_name;
94 | std::string help;
95 | std::function callback;
96 | };
97 |
98 | struct option
99 | {
100 | option(std::string sname, std::string lname, std::string help, std::string type, std::string value)
101 | : short_name(std::move(sname)), long_name(std::move(lname)), help(std::move(help)), type(std::move(type)),
102 | value(std::move(value))
103 | {}
104 |
105 | std::string short_name;
106 | std::string long_name;
107 | std::string help;
108 | std::string type;
109 | std::string value;
110 | };
111 |
112 | struct argument
113 | {
114 | argument(std::string name, std::string help, std::string type)
115 | : name(std::move(name)), help(std::move(help)), type(std::move(type))
116 | {}
117 |
118 | std::string name;
119 | std::string help;
120 | std::string type;
121 | std::string value;
122 | };
123 |
124 | class argparser
125 | {
126 | private:
127 | std::string description;
128 | std::string program_name;
129 | std::vector short_circuit_options;
130 | std::vector options;
131 | std::unordered_map short_name_index;
132 | std::vector named_arguments;
133 | std::vector arguments;
134 |
135 | public:
136 | argparser(std::string description) : description(std::move(description)) {}
137 |
138 | argparser &set_program_name(std::string name)
139 | {
140 | program_name = std::move(name);
141 | return *this;
142 | }
143 |
144 | void print_usage() const
145 | {
146 | std::cout << "usage: " << program_name << " [options]";
147 | for (const auto &named_arg : named_arguments)
148 | {
149 | std::cout << " [=" << named_arg.name << "]";
150 | }
151 | for (const auto &arg : arguments)
152 | {
153 | std::cout << " [" << arg.name << "]";
154 | }
155 | std::cout << std::endl;
156 | }
157 |
158 | void print_help() const
159 | {
160 | print_usage();
161 | std::cout << "\n" << description << "\n\n";
162 | std::cout << "Options:\n";
163 | // calculate the longest option name
164 | std::size_t max_name_length = 0;
165 | for (const auto &opt : short_circuit_options)
166 | {
167 | std::size_t length = opt.long_name.length();
168 | if (!opt.short_name.empty())
169 | {
170 | length += 4;
171 | }
172 | max_name_length = std::max(max_name_length, length);
173 | }
174 | for (const auto &opt : options)
175 | {
176 | std::size_t length = opt.long_name.length();
177 | if (!opt.short_name.empty())
178 | {
179 | length += 4;
180 | }
181 | max_name_length = std::max(max_name_length, length);
182 | }
183 | max_name_length = std::max(max_name_length, std::size_t(25));
184 | // print the options
185 | for (const auto &opt : short_circuit_options)
186 | {
187 | std::cout << " ";
188 | std::size_t printed_length = 0;
189 | if (!opt.short_name.empty())
190 | {
191 | std::cout << opt.short_name << ", ";
192 | printed_length = 4;
193 | }
194 | std::cout << opt.long_name;
195 | printed_length += opt.long_name.length();
196 | std::cout << std::string(max_name_length - printed_length, ' ');
197 | std::cout << replace(opt.help, "\n", "\n" + std::string(max_name_length + 2, ' ')) << '\n';
198 | }
199 | for (const auto &opt : options)
200 | {
201 | std::cout << " ";
202 | std::size_t printed_length = 0;
203 | if (!opt.short_name.empty())
204 | {
205 | std::cout << opt.short_name << ", ";
206 | printed_length = 4;
207 | }
208 | std::cout << opt.long_name;
209 | printed_length += opt.long_name.length();
210 | std::cout << std::string(max_name_length - printed_length, ' ');
211 | if (opt.type != "bool")
212 | {
213 | std::cout << "(" << opt.type << ") ";
214 | }
215 | std::cout << replace(opt.help, "\n", "\n" + std::string(max_name_length + 2, ' ')) << '\n';
216 | }
217 | if (named_arguments.size() > 0)
218 | {
219 | std::cout << "\nNamed arguments:\n";
220 | max_name_length = 0;
221 | for (const auto &arg : named_arguments)
222 | {
223 | max_name_length = std::max(max_name_length, arg.name.length());
224 | }
225 | max_name_length = std::max(max_name_length, std::size_t(25));
226 | for (const auto &arg : named_arguments)
227 | {
228 | std::cout << " ";
229 | std::cout << arg.name;
230 | std::cout << std::string(max_name_length - arg.name.length(), ' ') << "(" << arg.type << ") ";
231 | std::cout << replace(arg.help, "\n", "\n" + std::string(max_name_length + 2, ' ')) << '\n';
232 | }
233 | }
234 | if (arguments.size() > 0)
235 | {
236 | std::cout << "\nPosition arguments:\n";
237 | max_name_length = 0;
238 | for (const auto &arg : arguments)
239 | {
240 | max_name_length = std::max(max_name_length, arg.name.length());
241 | }
242 | max_name_length = std::max(max_name_length, std::size_t(25));
243 | for (const auto &arg : arguments)
244 | {
245 | std::cout << " ";
246 | std::cout << arg.name;
247 | std::cout << std::string(max_name_length - arg.name.length(), ' ') << "(" << arg.type << ") ";
248 | std::cout << replace(arg.help, "\n", "\n" + std::string(max_name_length + 2, ' ')) << '\n';
249 | }
250 | }
251 | }
252 |
253 | argparser &add_help_option()
254 | {
255 | return add_sc_option("-?", "--help", "show this help message", [this]() { print_help(); });
256 | }
257 |
258 | // add short circuit option
259 | argparser &add_sc_option(std::string sname, std::string lname, std::string help, std::function callback)
260 | {
261 | // long name must not be empty
262 | check_add_option_lname(lname);
263 | // allow short name to be empty
264 | if (sname != "")
265 | {
266 | check_add_option_sname(sname);
267 | short_name_index[sname.back()] = short_circuit_options.size();
268 | }
269 | short_circuit_options.emplace_back(std::move(sname), std::move(lname), std::move(help), std::move(callback));
270 | return *this;
271 | }
272 |
273 | template
274 | argparser &add_option(std::string sname, std::string lname, std::string help, T &&default_value)
275 | {
276 | if (type_string() == "null")
277 | {
278 | std::cerr << "(build error) unsupport type for option: " << typeid(T).name() << std::endl;
279 | std::exit(-1);
280 | }
281 | check_add_option_lname(lname);
282 | if (sname != "")
283 | {
284 | check_add_option_sname(sname);
285 | short_name_index[sname.back()] = options.size();
286 | }
287 | options.emplace_back(std::move(sname), std::move(lname), std::move(help), type_string(),
288 | to_string(default_value));
289 | return *this;
290 | }
291 |
292 | argparser &add_option(std::string sname, std::string lname, std::string help)
293 | {
294 | check_add_option_lname(lname);
295 | if (sname != "")
296 | {
297 | check_add_option_sname(sname);
298 | short_name_index[sname.back()] = options.size();
299 | }
300 | options.emplace_back(std::move(sname), std::move(lname), std::move(help), "bool", "0");
301 | return *this;
302 | }
303 |
304 | template
305 | argparser &add_argument(std::string name, std::string help)
306 | {
307 | check_add_argument_name(name);
308 | arguments.emplace_back(std::move(name), std::move(help), type_string());
309 | return *this;
310 | }
311 |
312 | template
313 | argparser &add_named_argument(std::string name, std::string help)
314 | {
315 | check_add_argument_name(name);
316 | named_arguments.emplace_back(std::move(name), std::move(help), type_string());
317 | return *this;
318 | }
319 |
320 | template
321 | T get_option(const std::string &name) const
322 | {
323 | auto pos = find_option_sname(name);
324 | if (pos == options.cend())
325 | {
326 | pos = find_option_lname(name);
327 | }
328 | if (pos == options.cend())
329 | {
330 | std::cerr << "(get error) option not found: " << name << std::endl;
331 | std::exit(-1);
332 | }
333 | if (pos->type != type_string())
334 | {
335 | std::cerr << "(get error) option type mismatch: set '" << pos->type << "' but you try get with '"
336 | << type_string() << "'" << std::endl;
337 | std::exit(-1);
338 | }
339 | return parse_value(pos->value);
340 | }
341 |
342 | // some alias for get_option
343 | bool has_option(const std::string &name) const { return get_option(name); }
344 | bool get_option_bool(const std::string &name) const { return get_option(name); }
345 | int get_option_int(const std::string &name) const { return get_option(name); }
346 | int64_t get_option_int64(const std::string &name) const { return get_option(name); }
347 | double get_option_double(const std::string &name) const { return get_option(name); }
348 | std::string get_option_string(const std::string &name) const { return get_option(name); }
349 |
350 | template
351 | T get_argument(const std::string &name) const
352 | {
353 | auto pos = find_argument(name);
354 | if (pos == arguments.cend())
355 | {
356 | pos = find_named_argument(name);
357 | }
358 | if (pos == named_arguments.cend())
359 | {
360 | std::cerr << "(get error) argument not found: " << name << std::endl;
361 | std::exit(-1);
362 | }
363 | if (pos->type != type_string())
364 | {
365 | std::cerr << "(get error) argument type mismatch: set '" << pos->type << "' but you try get with '"
366 | << type_string() << "'" << std::endl;
367 | std::exit(-1);
368 | }
369 | return parse_value(pos->value);
370 | }
371 |
372 | // some alias for get_argument
373 | int get_argument_int(const std::string &name) const { return get_argument(name); }
374 | int64_t get_argument_int64(const std::string &name) const { return get_argument(name); }
375 | double get_argument_double(const std::string &name) const { return get_argument(name); }
376 | std::string get_argument_string(const std::string &name) const { return get_argument(name); }
377 |
378 | // parse arguments
379 | argparser &parse(int argc, char *argv[])
380 | {
381 | // if not set program name, use argv[0]
382 | if (program_name == "")
383 | {
384 | program_name = argv[0];
385 | }
386 | if (argc == 1)
387 | {
388 | if (arguments.size()) {
389 | print_usage();
390 | std::exit(0);
391 | }
392 | }
393 | std::vector tokens;
394 | for (int i = 1; i < argc; ++i)
395 | {
396 | tokens.emplace_back(argv[i]);
397 | }
398 | // start parse short circuit options
399 | for (auto &&sc_opt : short_circuit_options)
400 | {
401 | auto pos = std::find_if(tokens.cbegin(), tokens.cend(),
402 | [&sc_opt](const std::string &tok)
403 | { return tok == sc_opt.short_name || tok == sc_opt.long_name; });
404 | if (pos != tokens.cend())
405 | {
406 | sc_opt.callback();
407 | std::exit(0);
408 | }
409 | }
410 | // start parse options
411 | for (auto &&opt : options)
412 | {
413 | auto pos =
414 | std::find_if(tokens.cbegin(), tokens.cend(),
415 | [&opt](const std::string &tok) { return tok == opt.short_name || tok == opt.long_name; });
416 | if (pos == tokens.cend())
417 | {
418 | continue;
419 | }
420 | pos = tokens.erase(pos);
421 | if (opt.type == "bool")
422 | {
423 | opt.value = "1";
424 | }
425 | else // other types need to parse next token
426 | {
427 | if (pos == tokens.cend())
428 | {
429 | std::cerr << "(parse error) option " << opt.short_name << " " << opt.long_name
430 | << " should have value" << std::endl;
431 | std::exit(-1);
432 | }
433 | opt.value = *pos;
434 | pos = tokens.erase(pos);
435 | }
436 | }
437 | // if there are short name like options, parse it as aggregation short name options
438 | {
439 | auto pos =
440 | std::find_if(tokens.cbegin(), tokens.cend(), [](const std::string &tok) { return tok.front() == '-'; });
441 | if (pos != tokens.cend())
442 | {
443 | if (pos->length() == 1)
444 | {
445 | std::cerr << "(parse error) bare unexcepted '-'" << std::endl;
446 | std::exit(-1);
447 | }
448 | if ((*pos)[1] == '-')
449 | {
450 | std::cerr << "(parse error) unrecognized option" << (*pos) << std::endl;
451 | std::exit(-1);
452 | }
453 | std::string short_names = pos->substr(1);
454 | for (char ch : short_names)
455 | {
456 | std::size_t index = short_name_index[ch];
457 | if (index < short_circuit_options.size() && short_circuit_options[index].short_name.back() == ch)
458 | {
459 | short_circuit_options[index].callback();
460 | std::exit(0);
461 | }
462 | }
463 | for (char ch : short_names)
464 | {
465 | std::size_t index = short_name_index[ch];
466 | if (index < options.size() && options[index].short_name.back() == ch)
467 | {
468 | if (options[index].type != "bool")
469 | {
470 | std::cerr << "(parse error) aggregation short name option must be bool" << std::endl;
471 | std::exit(-1);
472 | }
473 | options[index].value = "1";
474 | }
475 | else
476 | {
477 | std::cerr << "(parse error) unrecognized short name option '" << ch << "' in " << (*pos)
478 | << std::endl;
479 | std::exit(-1);
480 | }
481 | }
482 | pos = tokens.erase(pos);
483 | }
484 | }
485 | // start parse named arguments
486 | if (tokens.size() < named_arguments.size())
487 | {
488 | std::cerr << "(parse error) not enough named_arguments" << std::endl;
489 | std::exit(-1);
490 | }
491 | for (auto &named_arg : named_arguments)
492 | {
493 | for (auto pos = tokens.begin(); pos != tokens.end();)
494 | {
495 | if (try_parse_named_argument(*pos, named_arg))
496 | {
497 | pos = tokens.erase(pos);
498 | break;
499 | }
500 | ++pos;
501 | }
502 | if (named_arg.value == "")
503 | {
504 | std::cerr << "(parse error) named_argument " << named_arg.name << " should have value" << std::endl;
505 | std::exit(-1);
506 | }
507 | }
508 | // start parse position arguments
509 | if (tokens.size() != arguments.size())
510 | {
511 | std::cerr << "(parse error) position argument number missmatching, give " << tokens.size() << ", but need "
512 | << arguments.size() << '\n';
513 | std::cerr << "uncaptured command line arguments:\n";
514 | for (const auto &tok : tokens)
515 | {
516 | std::cerr << tok << '\n';
517 | }
518 | std::cerr << std::flush;
519 | std::exit(-1);
520 | }
521 | for (std::size_t i = 0; i < tokens.size(); ++i)
522 | {
523 | arguments[i].value = tokens[i];
524 | }
525 | return *this;
526 | }
527 |
528 | // print to file
529 | void print_as_ini(std::ostream &os, bool comments = false) const
530 | {
531 | if (options.size() > 0)
532 | {
533 | os << "[options]\n";
534 | }
535 | for (const auto &opt : options)
536 | {
537 | if (comments)
538 | {
539 | os << "# " << replace(opt.help, "\n", "\n# ") << "\n";
540 | }
541 | if (opt.type == "bool")
542 | {
543 | os << opt.long_name.substr(2) << "=" << (opt.value == "1" ? "true" : "false") << "\n";
544 | }
545 | else
546 | {
547 | os << opt.long_name.substr(2) << "=" << opt.value << '\n';
548 | }
549 | }
550 | if (named_arguments.size() > 0)
551 | {
552 | os << "[named_arguments]\n";
553 | }
554 | for (const auto &named_arg : named_arguments)
555 | {
556 | if (comments)
557 | {
558 | os << "# " << replace(named_arg.help, "\n", "\n# ") << "\n";
559 | }
560 | os << named_arg.name << "=" << named_arg.value << '\n';
561 | }
562 | if (arguments.size() > 0)
563 | {
564 | os << "[arguments]\n";
565 | }
566 | for (const auto &arg : arguments)
567 | {
568 | if (comments)
569 | {
570 | os << "# " << replace(arg.help, "\n", "\n# ") << "\n";
571 | }
572 | os << arg.name << "=" << arg.value << '\n';
573 | }
574 | }
575 |
576 | private:
577 | bool try_parse_named_argument(const std::string &line, argument &named_arg)
578 | {
579 | auto pos = line.find('=');
580 | if (pos == std::string::npos)
581 | {
582 | return false;
583 | }
584 | auto name = line.substr(0, pos);
585 | auto value = line.substr(pos + 1);
586 | if (name != named_arg.name)
587 | {
588 | return false;
589 | }
590 | else
591 | {
592 | named_arg.value = value;
593 | return true;
594 | }
595 | }
596 |
597 | std::string replace(const std::string &str, const std::string &from, const std::string &to) const
598 | {
599 | std::string ret;
600 | std::size_t pos = 0, pre_pos = 0;
601 | while ((pos = str.find(from, pre_pos)) != std::string::npos)
602 | {
603 | ret += str.substr(pre_pos, pos - pre_pos) + to;
604 | pre_pos = pos + from.length();
605 | }
606 | ret += str.substr(pre_pos);
607 | return ret;
608 | }
609 |
610 | using argument_iterator = std::vector::const_iterator;
611 | using option_iterator = std::vector::const_iterator;
612 | using sc_option_iterator = std::vector::const_iterator;
613 |
614 | auto find_argument(const std::string &key) const -> argument_iterator
615 | {
616 | return std::find_if(arguments.cbegin(), arguments.cend(),
617 | [&key](const argument &arg) { return arg.name == key; });
618 | }
619 |
620 | auto find_named_argument(const std::string &key) const -> argument_iterator
621 | {
622 | return std::find_if(named_arguments.cbegin(), named_arguments.cend(),
623 | [&key](const argument &arg) { return arg.name == key; });
624 | }
625 |
626 | auto find_sc_option_sname(const std::string &key) const -> sc_option_iterator
627 | {
628 | return std::find_if(short_circuit_options.cbegin(), short_circuit_options.cend(),
629 | [&key](const short_circuit_option &opt) { return opt.short_name == key; });
630 | }
631 |
632 | auto find_sc_option_lname(const std::string &key) const -> sc_option_iterator
633 | {
634 | return std::find_if(short_circuit_options.cbegin(), short_circuit_options.cend(),
635 | [&key](const short_circuit_option &opt) { return opt.long_name == key; });
636 | }
637 |
638 | auto find_option_sname(const std::string &key) const -> option_iterator
639 | {
640 | return std::find_if(options.cbegin(), options.cend(),
641 | [&key](const option &opt) { return opt.short_name == key; });
642 | }
643 |
644 | auto find_option_lname(const std::string &key) const -> option_iterator
645 | {
646 | return std::find_if(options.cbegin(), options.cend(),
647 | [&key](const option &opt) { return opt.long_name == key; });
648 | }
649 |
650 | void check_add_option_sname(const std::string &key) const
651 | {
652 | // std::cout << key << std::endl;
653 | if (key.size() > 6 || key.front() != '-')
654 | {
655 | std::cerr << "(build error) short option name must be `-` followed by no more than 5 character" << std::endl;
656 | std::exit(-1);
657 | }
658 | const char *ch = key.c_str();
659 | // std::cout << ch << std::endl;
660 | if (short_name_index.find(*ch) != short_name_index.end())
661 | {
662 | std::cerr << "(build error) short option name " << key << " already exists" << std::endl;
663 | std::exit(-1);
664 | }
665 | }
666 |
667 | void check_add_option_lname(const std::string &key) const
668 | {
669 | if (key == "")
670 | {
671 | std::cerr << "(build error) long option name cannot be empty" << std::endl;
672 | std::exit(-1);
673 | }
674 | if (key.substr(0, 2) != "--")
675 | {
676 | std::cerr << "(build error) long option name must be `--` followed by one or more characters" << std::endl;
677 | std::exit(-1);
678 | }
679 | if (find_option_lname(key) != options.cend() || find_sc_option_lname(key) != short_circuit_options.cend())
680 | {
681 | std::cerr << "(build error) long option name " << key << " already exists" << std::endl;
682 | std::exit(-1);
683 | }
684 | }
685 |
686 | template
687 | void check_add_argument_name(const std::string &key) const
688 | {
689 | if (type_string() == "null")
690 | {
691 | std::cerr << "(build error) argument type is not supported: " << typeid(T).name() << std::endl;
692 | std::exit(-1);
693 | }
694 | if (type_string() == "bool")
695 | {
696 | std::cerr << "(build error) argument type cannot be bool" << std::endl;
697 | std::exit(-1);
698 | }
699 | if (key == "")
700 | {
701 | std::cerr << "(build error) argument name cannot be empty" << std::endl;
702 | std::exit(-1);
703 | }
704 | if (find_argument(key) != arguments.cend() || find_named_argument(key) != named_arguments.cend())
705 | {
706 | std::cerr << "(build error) argument name " << key << " already exists" << std::endl;
707 | std::exit(-1);
708 | }
709 | }
710 | };
711 |
712 | }; // namespace util
713 |
714 | #endif // JSHL_ARGPARSE_HPP
715 |
--------------------------------------------------------------------------------
/include/image_utils/detect_process.h:
--------------------------------------------------------------------------------
1 | #ifndef DETECT_PROCESS_H
2 | #define DETECT_PROCESS_H
3 |
4 | #include
5 |
6 | namespace detect {
7 |
8 | const int color_list[][3] = {
9 | {255, 56, 56},
10 | {255, 157, 151},
11 | {255, 112, 31},
12 | {255, 178, 29},
13 | {207, 210, 49},
14 | {72, 249, 10},
15 | {146, 204, 23},
16 | {61, 219, 134},
17 | {26, 147, 52},
18 | {0, 212, 187},
19 | {44, 153, 168},
20 | {0, 194, 255},
21 | {52, 69, 147},
22 | {100, 115, 255},
23 | {0, 24, 236},
24 | {132, 56, 255},
25 | {82, 0, 133},
26 | {203, 56, 255},
27 | {255, 149, 200},
28 | {255, 55, 198}
29 | };
30 |
31 | struct Object
32 | {
33 | cv::Rect_ rect;
34 | int label;
35 | float prob;
36 | };
37 |
38 | struct resizeInfo
39 | {
40 | cv::Mat resized_img;
41 | float factor;
42 | };
43 |
44 | static cv::Scalar get_color(int index){
45 | index %= 20;
46 | return cv::Scalar(color_list[index][2], color_list[index][1], color_list[index][0]);
47 | }
48 |
49 | static cv::Mat static_resize(cv::Mat& img, cv::Size input_size) {
50 | float r = std::min(input_size.width / (img.cols*1.0), input_size.height / (img.rows*1.0));
51 | int unpad_w = r * img.cols;
52 | int unpad_h = r * img.rows;
53 |
54 | cv::Mat re(unpad_h, unpad_w, CV_8UC3);
55 | cv::resize(img, re, re.size());
56 | cv::Mat out(input_size.height, input_size.width, CV_8UC3, cv::Scalar(114, 114, 114));
57 | re.copyTo(out(cv::Rect(0, 0, re.cols, re.rows)));
58 | return out;
59 | }
60 |
61 | static resizeInfo resizeAndPad(cv::Mat &src_img, cv::Size _size, bool center=false, bool show=false)
62 | {
63 | int img_C = src_img.channels();
64 | int img_H = src_img.rows;
65 | int img_W = src_img.cols;
66 | cv::Mat resize_img;
67 |
68 | resizeInfo info;
69 |
70 | float factor_h = (double)img_H / _size.height;
71 | float factor_w = (double)img_W / _size.width;
72 |
73 | // std::cout << factor_h << "," << factor_w << std::endl;
74 |
75 | if (factor_h >= factor_w)
76 | {
77 | info.factor = factor_h;
78 | int new_W = int(img_W / factor_h);
79 | if (new_W % 2 != 0) new_W -= 1;
80 | cv::resize(src_img.clone(), resize_img, cv::Size(new_W, _size.height));
81 | int img_C2 = resize_img.channels();
82 | int img_H2 = resize_img.rows;
83 | int img_W2 = resize_img.cols;
84 |
85 | int pad_w0 = 0;
86 | int pad_w1 = _size.width - new_W;
87 | if (center) {
88 | pad_w1 /= 2;
89 | pad_w0 = pad_w1;
90 | }
91 |
92 | cv::copyMakeBorder(resize_img, info.resized_img, 0, 0, pad_w0, pad_w1, cv::BORDER_CONSTANT, cv::Scalar(114, 114, 114));
93 | }
94 | else
95 | {
96 | info.factor = factor_w;
97 | int new_H = int(img_H / factor_w);
98 | if (new_H % 2 != 0) new_H -= 1;
99 |
100 | cv::resize(src_img.clone(), resize_img, cv::Size(_size.width, new_H));
101 | int img_C2 = resize_img.channels();
102 | int img_H2 = resize_img.rows;
103 | int img_W2 = resize_img.cols;
104 |
105 | int pad_h0 = 0;
106 | int pad_h1 = _size.height - new_H;
107 | if (center) {
108 | pad_h1 /= 2;
109 | pad_h0 = pad_h1;
110 | }
111 |
112 | cv::copyMakeBorder(resize_img, info.resized_img, pad_h0, pad_h1, 0, 0, cv::BORDER_CONSTANT, cv::Scalar(114, 114, 114));
113 | }
114 | if (show) {
115 | cv::imshow("resize pad image", info.resized_img);
116 | cv::waitKey(0);
117 | }
118 | return info;
119 | }
120 |
121 | static cv::Mat draw_boxes(cv::Mat image,
122 | std::vector &objects,
123 | std::vector& names,
124 | int draw_size=20,
125 | bool draw_label=true) {
126 | cv::Mat d_img = image.clone();
127 | cv::Scalar color;
128 | cv::Scalar txt_color;
129 | cv::Scalar txt_bk_color;
130 | cv::Size label_size;
131 | int baseLine = 0;
132 | int x, y, out_point_y, w, h;
133 | int line_thickness = std::round((double)draw_size / 10.0);
134 |
135 | for(int k=0; k 127)?cv::Scalar(0, 0, 0):cv::Scalar(255, 255, 255);
150 | std::string label = names.at(objects.at(k).label) + " " + std::to_string(objects.at(k).prob).substr(0, 4);
151 | label_size = cv::getTextSize(label.c_str(), cv::LINE_AA, double(draw_size) / 30.0, (line_thickness>1)?line_thickness-1:1, &baseLine);
152 | txt_bk_color = color; // * 0.7;
153 | y = (y > d_img.rows)?d_img.rows:y + 1;
154 | out_point_y = y - label_size.height - baseLine;
155 | if (out_point_y >= 0) y = out_point_y;
156 | cv::rectangle(d_img, cv::Rect(cv::Point(x - (line_thickness - 1), y), cv::Size(label_size.width, label_size.height + baseLine)),
157 | txt_bk_color, -1);
158 | cv::putText(d_img, label, cv::Point(x, y + label_size.height),
159 | cv::LINE_AA, double(draw_size) / 30.0, txt_color, (line_thickness>1)?line_thickness-1:1);
160 | }
161 |
162 | }
163 | return d_img;
164 | }
165 |
166 | static void qsort_descent_inplace(std::vector& faceobjects, int left, int right) {
167 | int i = left;
168 | int j = right;
169 | float p = faceobjects[(left + right) / 2].prob;
170 |
171 | while (i <= j)
172 | {
173 | while (faceobjects[i].prob > p)
174 | i++;
175 |
176 | while (faceobjects[j].prob < p)
177 | j--;
178 |
179 | if (i <= j)
180 | {
181 | std::swap(faceobjects[i], faceobjects[j]);
182 | i++;
183 | j--;
184 | }
185 | }
186 |
187 | if (left < j) qsort_descent_inplace(faceobjects, left, j);
188 | if (i < right) qsort_descent_inplace(faceobjects, i, right);
189 |
190 | }
191 |
192 | static void qsort_descent_inplace(std::vector& objects) {
193 | if (objects.empty())
194 | return;
195 |
196 | qsort_descent_inplace(objects, 0, objects.size() - 1);
197 | }
198 |
199 | static inline float intersection_area(const Object& a, const Object& b) {
200 | cv::Rect_ inter = a.rect & b.rect;
201 | return inter.area();
202 | }
203 |
204 | static void nms_sorted_bboxes(const std::vector& faceobjects, std::vector& picked, float nms_threshold)
205 | {
206 | picked.clear();
207 |
208 | const int n = faceobjects.size();
209 |
210 | std::vector areas(n);
211 | for (int i = 0; i < n; i++) areas[i] = faceobjects[i].rect.area();
212 |
213 | for (int i = 0; i < n; i++) {
214 | const Object& a = faceobjects[i];
215 |
216 | bool keep = true;
217 | for (int j = 0; j < (int)picked.size(); j++) {
218 | const Object& b = faceobjects[picked[j]];
219 | float inter_area = intersection_area(a, b);
220 | float union_area = areas[i] + areas[picked[j]] - inter_area;
221 | // float IoU = inter_area / union_area
222 | if (inter_area / union_area > nms_threshold) {
223 | keep = false;
224 | break;
225 | }
226 | }
227 |
228 | if (keep) picked.push_back(i);
229 | }
230 | }
231 |
232 | }
233 |
234 |
235 | #endif
236 | #define DETECT_PROCESS_H
--------------------------------------------------------------------------------
/include/image_utils/lap/_lapjv.pyx:
--------------------------------------------------------------------------------
1 | # Tomas Kazmar, 2012-2017, BSD 2-clause license, see LICENSE.
2 |
3 | import numpy as np
4 | cimport numpy as cnp
5 | cimport cython
6 | from libc.stdlib cimport malloc, free
7 |
8 |
9 | cdef extern from "lapjv.h" nogil:
10 | ctypedef signed int int_t
11 | ctypedef unsigned int uint_t
12 | cdef int LARGE
13 | cdef enum fp_t:
14 | FP_1
15 | FP_2
16 | FP_DYNAMIC
17 | int lapjv_internal(
18 | const uint_t n,
19 | double *cost[],
20 | int_t *x,
21 | int_t *y)
22 | int lapmod_internal(
23 | const uint_t n,
24 | double *cc,
25 | uint_t *ii,
26 | uint_t *kk,
27 | int_t *x,
28 | int_t *y,
29 | fp_t fp_version)
30 |
31 | LARGE_ = LARGE
32 | FP_1_ = FP_1
33 | FP_2_ = FP_2
34 | FP_DYNAMIC_ = FP_DYNAMIC
35 |
36 |
37 | @cython.boundscheck(False)
38 | @cython.wraparound(False)
39 | def lapjv(cnp.ndarray cost not None, char extend_cost=False,
40 | double cost_limit=np.inf, char return_cost=True):
41 | """Solve linear assignment problem using Jonker-Volgenant algorithm.
42 |
43 | Parameters
44 | ----------
45 | cost: (N,N) ndarray
46 | Cost matrix. Entry `cost[i, j]` is the cost of assigning row `i` to
47 | column `j`.
48 | extend_cost: bool, optional
49 | Whether or not extend a non-square matrix. Default: False.
50 | cost_limit: double, optional
51 | An upper limit for a cost of a single assignment. Default: `np.inf`.
52 | return_cost: bool, optional
53 | Whether or not to return the assignment cost.
54 |
55 | Returns
56 | -------
57 | opt: double
58 | Assignment cost. Not returned if `return_cost is False`.
59 | x: (N,) ndarray
60 | Assignment. `x[i]` specifies the column to which row `i` is assigned.
61 | y: (N,) ndarray
62 | Assignment. `y[j]` specifies the row to which column `j` is assigned.
63 |
64 | Notes
65 | -----
66 | For non-square matrices (with `extend_cost is True`) or `cost_limit` set
67 | low enough, there will be unmatched rows, columns in the solution `x`, `y`.
68 | All such entries are set to -1.
69 | """
70 | if cost.ndim != 2:
71 | raise ValueError('2-dimensional array expected')
72 | cdef cnp.ndarray[cnp.double_t, ndim=2, mode='c'] cost_c = \
73 | np.ascontiguousarray(cost, dtype=np.double)
74 | cdef cnp.ndarray[cnp.double_t, ndim=2, mode='c'] cost_c_extended
75 | cdef uint_t n_rows = cost_c.shape[0]
76 | cdef uint_t n_cols = cost_c.shape[1]
77 | cdef uint_t n = 0
78 | if n_rows == n_cols:
79 | n = n_rows
80 | else:
81 | if not extend_cost:
82 | raise ValueError(
83 | 'Square cost array expected. If cost is intentionally '
84 | 'non-square, pass extend_cost=True.')
85 | if extend_cost or cost_limit < np.inf:
86 | n = n_rows + n_cols
87 | cost_c_extended = np.empty((n, n), dtype=np.double)
88 | if cost_limit < np.inf:
89 | cost_c_extended[:] = cost_limit / 2.
90 | else:
91 | cost_c_extended[:] = cost_c.max() + 1
92 | cost_c_extended[n_rows:, n_cols:] = 0
93 | cost_c_extended[:n_rows, :n_cols] = cost_c
94 | cost_c = cost_c_extended
95 |
96 | cdef double **cost_ptr
97 | cost_ptr = malloc(n * sizeof(double *))
98 | cdef int i
99 | for i in range(n):
100 | cost_ptr[i] = &cost_c[i, 0]
101 |
102 | cdef cnp.ndarray[int_t, ndim=1, mode='c'] x_c = \
103 | np.empty((n,), dtype=np.int32)
104 | cdef cnp.ndarray[int_t, ndim=1, mode='c'] y_c = \
105 | np.empty((n,), dtype=np.int32)
106 |
107 | cdef int ret = lapjv_internal(n, cost_ptr, &x_c[0], &y_c[0])
108 | free(cost_ptr)
109 | if ret != 0:
110 | if ret == -1:
111 | raise MemoryError('Out of memory.')
112 | raise RuntimeError('Unknown error (lapjv_internal returned %d).' % ret)
113 |
114 |
115 | cdef double opt = np.nan
116 | if n != n_rows:
117 | x_c[x_c >= n_cols] = -1
118 | y_c[y_c >= n_rows] = -1
119 | x_c = x_c[:n_rows]
120 | y_c = y_c[:n_cols]
121 | if return_cost:
122 | opt = cost_c[np.nonzero(x_c != -1)[0], x_c[x_c != -1]].sum()
123 | elif return_cost:
124 | opt = cost_c[np.arange(n_rows), x_c].sum()
125 |
126 | if return_cost:
127 | return opt, x_c, y_c
128 | else:
129 | return x_c, y_c
130 |
131 |
132 | @cython.boundscheck(False)
133 | @cython.wraparound(False)
134 | def _lapmod(
135 | const uint_t n,
136 | cnp.ndarray cc not None,
137 | cnp.ndarray ii not None,
138 | cnp.ndarray kk not None,
139 | fp_t fp_version=FP_DYNAMIC):
140 | """Internal function called from lapmod(..., fast=True)."""
141 | cdef cnp.ndarray[cnp.double_t, ndim=1, mode='c'] cc_c = \
142 | np.ascontiguousarray(cc, dtype=np.double)
143 | cdef cnp.ndarray[uint_t, ndim=1, mode='c'] ii_c = \
144 | np.ascontiguousarray(ii, dtype=np.uint32)
145 | cdef cnp.ndarray[uint_t, ndim=1, mode='c'] kk_c = \
146 | np.ascontiguousarray(kk, dtype=np.uint32)
147 | cdef cnp.ndarray[int_t, ndim=1, mode='c'] x_c = \
148 | np.empty((n,), dtype=np.int32)
149 | cdef cnp.ndarray[int_t, ndim=1, mode='c'] y_c = \
150 | np.empty((n,), dtype=np.int32)
151 |
152 | cdef int_t ret = lapmod_internal(
153 | n, &cc_c[0], &ii_c[0], &kk_c[0],
154 | &x_c[0], &y_c[0], fp_version)
155 | if ret != 0:
156 | if ret == -1:
157 | raise MemoryError('Out of memory.')
158 | raise RuntimeError('Unknown error (lapmod_internal returned %d).' % ret)
159 |
160 | return x_c, y_c
161 |
--------------------------------------------------------------------------------
/include/image_utils/lap/lapjv.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include
4 |
5 | #include "lapjv.h"
6 |
7 | /** Column-reduction and reduction transfer for a dense cost matrix.
8 | */
9 | int_t _ccrrt_dense(const uint_t n, cost_t *cost[],
10 | int_t *free_rows, int_t *x, int_t *y, cost_t *v)
11 | {
12 | int_t n_free_rows;
13 | boolean *unique;
14 |
15 | for (uint_t i = 0; i < n; i++) {
16 | x[i] = -1;
17 | v[i] = LARGE;
18 | y[i] = 0;
19 | }
20 | for (uint_t i = 0; i < n; i++) {
21 | for (uint_t j = 0; j < n; j++) {
22 | const cost_t c = cost[i][j];
23 | if (c < v[j]) {
24 | v[j] = c;
25 | y[j] = i;
26 | }
27 | PRINTF("i=%d, j=%d, c[i,j]=%f, v[j]=%f y[j]=%d\n", i, j, c, v[j], y[j]);
28 | }
29 | }
30 | PRINT_COST_ARRAY(v, n);
31 | PRINT_INDEX_ARRAY(y, n);
32 | NEW(unique, boolean, n);
33 | memset(unique, TRUE, n);
34 | {
35 | int_t j = n;
36 | do {
37 | j--;
38 | const int_t i = y[j];
39 | if (x[i] < 0) {
40 | x[i] = j;
41 | } else {
42 | unique[i] = FALSE;
43 | y[j] = -1;
44 | }
45 | } while (j > 0);
46 | }
47 | n_free_rows = 0;
48 | for (uint_t i = 0; i < n; i++) {
49 | if (x[i] < 0) {
50 | free_rows[n_free_rows++] = i;
51 | } else if (unique[i]) {
52 | const int_t j = x[i];
53 | cost_t min = LARGE;
54 | for (uint_t j2 = 0; j2 < n; j2++) {
55 | if (j2 == (uint_t)j) {
56 | continue;
57 | }
58 | const cost_t c = cost[i][j2] - v[j2];
59 | if (c < min) {
60 | min = c;
61 | }
62 | }
63 | PRINTF("v[%d] = %f - %f\n", j, v[j], min);
64 | v[j] -= min;
65 | }
66 | }
67 | FREE(unique);
68 | return n_free_rows;
69 | }
70 |
71 |
72 | /** Augmenting row reduction for a dense cost matrix.
73 | */
74 | int_t _carr_dense(
75 | const uint_t n, cost_t *cost[],
76 | const uint_t n_free_rows,
77 | int_t *free_rows, int_t *x, int_t *y, cost_t *v)
78 | {
79 | uint_t current = 0;
80 | int_t new_free_rows = 0;
81 | uint_t rr_cnt = 0;
82 | PRINT_INDEX_ARRAY(x, n);
83 | PRINT_INDEX_ARRAY(y, n);
84 | PRINT_COST_ARRAY(v, n);
85 | PRINT_INDEX_ARRAY(free_rows, n_free_rows);
86 | while (current < n_free_rows) {
87 | int_t i0;
88 | int_t j1, j2;
89 | cost_t v1, v2, v1_new;
90 | boolean v1_lowers;
91 |
92 | rr_cnt++;
93 | PRINTF("current = %d rr_cnt = %d\n", current, rr_cnt);
94 | const int_t free_i = free_rows[current++];
95 | j1 = 0;
96 | v1 = cost[free_i][0] - v[0];
97 | j2 = -1;
98 | v2 = LARGE;
99 | for (uint_t j = 1; j < n; j++) {
100 | PRINTF("%d = %f %d = %f\n", j1, v1, j2, v2);
101 | const cost_t c = cost[free_i][j] - v[j];
102 | if (c < v2) {
103 | if (c >= v1) {
104 | v2 = c;
105 | j2 = j;
106 | } else {
107 | v2 = v1;
108 | v1 = c;
109 | j2 = j1;
110 | j1 = j;
111 | }
112 | }
113 | }
114 | i0 = y[j1];
115 | v1_new = v[j1] - (v2 - v1);
116 | v1_lowers = v1_new < v[j1];
117 | PRINTF("%d %d 1=%d,%f 2=%d,%f v1'=%f(%d,%g) \n", free_i, i0, j1, v1, j2, v2, v1_new, v1_lowers, v[j1] - v1_new);
118 | if (rr_cnt < current * n) {
119 | if (v1_lowers) {
120 | v[j1] = v1_new;
121 | } else if (i0 >= 0 && j2 >= 0) {
122 | j1 = j2;
123 | i0 = y[j2];
124 | }
125 | if (i0 >= 0) {
126 | if (v1_lowers) {
127 | free_rows[--current] = i0;
128 | } else {
129 | free_rows[new_free_rows++] = i0;
130 | }
131 | }
132 | } else {
133 | PRINTF("rr_cnt=%d >= %d (current=%d * n=%d)\n", rr_cnt, current * n, current, n);
134 | if (i0 >= 0) {
135 | free_rows[new_free_rows++] = i0;
136 | }
137 | }
138 | x[free_i] = j1;
139 | y[j1] = free_i;
140 | }
141 | return new_free_rows;
142 | }
143 |
144 |
145 | /** Find columns with minimum d[j] and put them on the SCAN list.
146 | */
147 | uint_t _find_dense(const uint_t n, uint_t lo, cost_t *d, int_t *cols, int_t *y)
148 | {
149 | uint_t hi = lo + 1;
150 | cost_t mind = d[cols[lo]];
151 | for (uint_t k = hi; k < n; k++) {
152 | int_t j = cols[k];
153 | if (d[j] <= mind) {
154 | if (d[j] < mind) {
155 | hi = lo;
156 | mind = d[j];
157 | }
158 | cols[k] = cols[hi];
159 | cols[hi++] = j;
160 | }
161 | }
162 | return hi;
163 | }
164 |
165 |
166 | // Scan all columns in TODO starting from arbitrary column in SCAN
167 | // and try to decrease d of the TODO columns using the SCAN column.
168 | int_t _scan_dense(const uint_t n, cost_t *cost[],
169 | uint_t *plo, uint_t*phi,
170 | cost_t *d, int_t *cols, int_t *pred,
171 | int_t *y, cost_t *v)
172 | {
173 | uint_t lo = *plo;
174 | uint_t hi = *phi;
175 | cost_t h, cred_ij;
176 |
177 | while (lo != hi) {
178 | int_t j = cols[lo++];
179 | const int_t i = y[j];
180 | const cost_t mind = d[j];
181 | h = cost[i][j] - v[j] - mind;
182 | PRINTF("i=%d j=%d h=%f\n", i, j, h);
183 | // For all columns in TODO
184 | for (uint_t k = hi; k < n; k++) {
185 | j = cols[k];
186 | cred_ij = cost[i][j] - v[j] - h;
187 | if (cred_ij < d[j]) {
188 | d[j] = cred_ij;
189 | pred[j] = i;
190 | if (cred_ij == mind) {
191 | if (y[j] < 0) {
192 | return j;
193 | }
194 | cols[k] = cols[hi];
195 | cols[hi++] = j;
196 | }
197 | }
198 | }
199 | }
200 | *plo = lo;
201 | *phi = hi;
202 | return -1;
203 | }
204 |
205 |
206 | /** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper.
207 | *
208 | * This is a dense matrix version.
209 | *
210 | * \return The closest free column index.
211 | */
212 | int_t find_path_dense(
213 | const uint_t n, cost_t *cost[],
214 | const int_t start_i,
215 | int_t *y, cost_t *v,
216 | int_t *pred)
217 | {
218 | uint_t lo = 0, hi = 0;
219 | int_t final_j = -1;
220 | uint_t n_ready = 0;
221 | int_t *cols;
222 | cost_t *d;
223 |
224 | NEW(cols, int_t, n);
225 | NEW(d, cost_t, n);
226 |
227 | for (uint_t i = 0; i < n; i++) {
228 | cols[i] = i;
229 | pred[i] = start_i;
230 | d[i] = cost[start_i][i] - v[i];
231 | }
232 | PRINT_COST_ARRAY(d, n);
233 | while (final_j == -1) {
234 | // No columns left on the SCAN list.
235 | if (lo == hi) {
236 | PRINTF("%d..%d -> find\n", lo, hi);
237 | n_ready = lo;
238 | hi = _find_dense(n, lo, d, cols, y);
239 | PRINTF("check %d..%d\n", lo, hi);
240 | PRINT_INDEX_ARRAY(cols, n);
241 | for (uint_t k = lo; k < hi; k++) {
242 | const int_t j = cols[k];
243 | if (y[j] < 0) {
244 | final_j = j;
245 | }
246 | }
247 | }
248 | if (final_j == -1) {
249 | PRINTF("%d..%d -> scan\n", lo, hi);
250 | final_j = _scan_dense(
251 | n, cost, &lo, &hi, d, cols, pred, y, v);
252 | PRINT_COST_ARRAY(d, n);
253 | PRINT_INDEX_ARRAY(cols, n);
254 | PRINT_INDEX_ARRAY(pred, n);
255 | }
256 | }
257 |
258 | PRINTF("found final_j=%d\n", final_j);
259 | PRINT_INDEX_ARRAY(cols, n);
260 | {
261 | const cost_t mind = d[cols[lo]];
262 | for (uint_t k = 0; k < n_ready; k++) {
263 | const int_t j = cols[k];
264 | v[j] += d[j] - mind;
265 | }
266 | }
267 |
268 | FREE(cols);
269 | FREE(d);
270 |
271 | return final_j;
272 | }
273 |
274 |
275 | /** Augment for a dense cost matrix.
276 | */
277 | int_t _ca_dense(
278 | const uint_t n, cost_t *cost[],
279 | const uint_t n_free_rows,
280 | int_t *free_rows, int_t *x, int_t *y, cost_t *v)
281 | {
282 | int_t *pred;
283 |
284 | NEW(pred, int_t, n);
285 |
286 | for (int_t *pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) {
287 | int_t i = -1, j;
288 | uint_t k = 0;
289 |
290 | PRINTF("looking at free_i=%d\n", *pfree_i);
291 | j = find_path_dense(n, cost, *pfree_i, y, v, pred);
292 | ASSERT(j >= 0);
293 | ASSERT(j < n);
294 | while (i != *pfree_i) {
295 | PRINTF("augment %d\n", j);
296 | PRINT_INDEX_ARRAY(pred, n);
297 | i = pred[j];
298 | PRINTF("y[%d]=%d -> %d\n", j, y[j], i);
299 | y[j] = i;
300 | PRINT_INDEX_ARRAY(x, n);
301 | SWAP_INDICES(j, x[i]);
302 | k++;
303 | if (k >= n) {
304 | ASSERT(FALSE);
305 | }
306 | }
307 | }
308 | FREE(pred);
309 | return 0;
310 | }
311 |
312 |
313 | /** Solve dense sparse LAP.
314 | */
315 | int lapjv_internal(
316 | const uint_t n, cost_t *cost[],
317 | int_t *x, int_t *y)
318 | {
319 | int ret;
320 | int_t *free_rows;
321 | cost_t *v;
322 |
323 | NEW(free_rows, int_t, n);
324 | NEW(v, cost_t, n);
325 | ret = _ccrrt_dense(n, cost, free_rows, x, y, v);
326 | int i = 0;
327 | while (ret > 0 && i < 2) {
328 | ret = _carr_dense(n, cost, ret, free_rows, x, y, v);
329 | i++;
330 | }
331 | if (ret > 0) {
332 | ret = _ca_dense(n, cost, ret, free_rows, x, y, v);
333 | }
334 | FREE(v);
335 | FREE(free_rows);
336 | return ret;
337 | }
338 |
--------------------------------------------------------------------------------
/include/image_utils/lap/lapjv.h:
--------------------------------------------------------------------------------
1 | #ifndef LAPJV_H
2 | #define LAPJV_H
3 |
4 | #define LARGE 1000000
5 |
6 | #if !defined TRUE
7 | #define TRUE 1
8 | #endif
9 | #if !defined FALSE
10 | #define FALSE 0
11 | #endif
12 |
13 | #define NEW(x, t, n) if ((x = (t *)malloc(sizeof(t) * (n))) == 0) { return -1; }
14 | #define FREE(x) if (x != 0) { free(x); x = 0; }
15 | #define SWAP_INDICES(a, b) { int_t _temp_index = a; a = b; b = _temp_index; }
16 |
17 | #if 0
18 | #include
19 | #define ASSERT(cond) assert(cond)
20 | #define PRINTF(fmt, ...) printf(fmt, ##__VA_ARGS__)
21 | #define PRINT_COST_ARRAY(a, n) \
22 | while (1) { \
23 | printf(#a" = ["); \
24 | if ((n) > 0) { \
25 | printf("%f", (a)[0]); \
26 | for (uint_t j = 1; j < n; j++) { \
27 | printf(", %f", (a)[j]); \
28 | } \
29 | } \
30 | printf("]\n"); \
31 | break; \
32 | }
33 | #define PRINT_INDEX_ARRAY(a, n) \
34 | while (1) { \
35 | printf(#a" = ["); \
36 | if ((n) > 0) { \
37 | printf("%d", (a)[0]); \
38 | for (uint_t j = 1; j < n; j++) { \
39 | printf(", %d", (a)[j]); \
40 | } \
41 | } \
42 | printf("]\n"); \
43 | break; \
44 | }
45 | #else
46 | #define ASSERT(cond)
47 | #define PRINTF(fmt, ...)
48 | #define PRINT_COST_ARRAY(a, n)
49 | #define PRINT_INDEX_ARRAY(a, n)
50 | #endif
51 |
52 |
53 | typedef signed int int_t;
54 | typedef unsigned int uint_t;
55 | typedef double cost_t;
56 | typedef char boolean;
57 | typedef enum fp_t { FP_1 = 1, FP_2 = 2, FP_DYNAMIC = 3 } fp_t;
58 |
59 | extern int_t lapjv_internal(
60 | const uint_t n, cost_t *cost[],
61 | int_t *x, int_t *y);
62 |
63 | extern int_t lapmod_internal(
64 | const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
65 | int_t *x, int_t *y, fp_t fp_version);
66 |
67 | #endif // LAPJV_H
68 |
--------------------------------------------------------------------------------
/include/image_utils/lap/lapmod.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include
4 |
5 | #include "lapjv.h"
6 |
7 | /** Column-reduction and reduction transfer for a sparse cost matrix.
8 | */
9 | int_t _ccrrt_sparse(const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
10 | int_t *free_rows, int_t *x, int_t *y, cost_t *v)
11 | {
12 | int_t n_free_rows;
13 | boolean *unique;
14 |
15 | for (uint_t i = 0; i < n; i++) {
16 | x[i] = -1;
17 | v[i] = LARGE;
18 | y[i] = 0;
19 | }
20 | for (uint_t i = 0; i < n; i++) {
21 | for (uint_t k = ii[i]; k < ii[i+1]; k++) {
22 | const int_t j = kk[k];
23 | const cost_t c = cc[k];
24 | if (c < v[j]) {
25 | v[j] = c;
26 | y[j] = i;
27 | }
28 | PRINTF("i=%d, k=%d, j=%d, c[i,j]=%f, v[j]=%f y[j]=%d\n", i, k, j, c, v[j], y[j]);
29 | }
30 | }
31 | PRINT_COST_ARRAY(v, n);
32 | PRINT_INDEX_ARRAY(y, n);
33 | NEW(unique, boolean, n);
34 | memset(unique, TRUE, n);
35 | {
36 | int_t j = n;
37 | do {
38 | j--;
39 | const int_t i = y[j];
40 | if (x[i] < 0) {
41 | x[i] = j;
42 | } else {
43 | unique[i] = FALSE;
44 | y[j] = -1;
45 | }
46 | } while (j > 0);
47 | }
48 | n_free_rows = 0;
49 | for (uint_t i = 0; i < n; i++) {
50 | if (x[i] < 0) {
51 | free_rows[n_free_rows++] = i;
52 | } else if (unique[i] && (ii[i+1] - ii[i] > 1)) {
53 | const int_t j = x[i];
54 | cost_t min = LARGE;
55 | for (uint_t k = ii[i]; k < ii[i+1]; k++) {
56 | const int_t j2 = kk[k];
57 | if (j2 == j) {
58 | continue;
59 | }
60 | const cost_t c = cc[k] - v[j2];
61 | if (c < min) {
62 | min = c;
63 | }
64 | }
65 | PRINTF("v[%d] = %f - %f\n", j, v[j], min);
66 | v[j] -= min;
67 | }
68 | }
69 | FREE(unique);
70 | return n_free_rows;
71 | }
72 |
73 |
74 | /** Augmenting row reduction for a sparse cost matrix.
75 | */
76 | int_t _carr_sparse(
77 | const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
78 | const uint_t n_free_rows,
79 | int_t *free_rows, int_t *x, int_t *y, cost_t *v)
80 | {
81 | uint_t current = 0;
82 | int_t new_free_rows = 0;
83 | uint_t rr_cnt = 0;
84 | PRINT_INDEX_ARRAY(x, n);
85 | PRINT_INDEX_ARRAY(y, n);
86 | PRINT_COST_ARRAY(v, n);
87 | PRINT_INDEX_ARRAY(free_rows, n_free_rows);
88 | while (current < n_free_rows) {
89 | int_t i0;
90 | int_t j1, j2;
91 | cost_t v1, v2, v1_new;
92 | boolean v1_lowers;
93 |
94 | rr_cnt++;
95 | PRINTF("current = %d rr_cnt = %d\n", current, rr_cnt);
96 | const int_t free_i = free_rows[current++];
97 | if (ii[free_i+1] - ii[free_i] > 0) {
98 | const uint_t k = ii[free_i];
99 | j1 = kk[k];
100 | v1 = cc[k] - v[j1];
101 | } else {
102 | j1 = 0;
103 | v1 = LARGE;
104 | }
105 | j2 = -1;
106 | v2 = LARGE;
107 | for (uint_t k = ii[free_i]+1; k < ii[free_i+1]; k++) {
108 | PRINTF("%d = %f %d = %f\n", j1, v1, j2, v2);
109 | const int_t j = kk[k];
110 | const cost_t c = cc[k] - v[j];
111 | if (c < v2) {
112 | if (c >= v1) {
113 | v2 = c;
114 | j2 = j;
115 | } else {
116 | v2 = v1;
117 | v1 = c;
118 | j2 = j1;
119 | j1 = j;
120 | }
121 | }
122 | }
123 | i0 = y[j1];
124 | v1_new = v[j1] - (v2 - v1);
125 | v1_lowers = v1_new < v[j1];
126 | PRINTF("%d %d 1=%d,%f 2=%d,%f v1'=%f(%d,%g) \n", free_i, i0, j1, v1, j2, v2, v1_new, v1_lowers, v[j1] - v1_new);
127 | if (rr_cnt < current * n) {
128 | if (v1_lowers) {
129 | v[j1] = v1_new;
130 | } else if (i0 >= 0 && j2 >= 0) {
131 | j1 = j2;
132 | i0 = y[j2];
133 | }
134 | if (i0 >= 0) {
135 | if (v1_lowers) {
136 | free_rows[--current] = i0;
137 | } else {
138 | free_rows[new_free_rows++] = i0;
139 | }
140 | }
141 | } else {
142 | PRINTF("rr_cnt=%d >= %d (current=%d * n=%d)\n", rr_cnt, current * n, current, n);
143 | if (i0 >= 0) {
144 | free_rows[new_free_rows++] = i0;
145 | }
146 | }
147 | x[free_i] = j1;
148 | y[j1] = free_i;
149 | }
150 | return new_free_rows;
151 | }
152 |
153 |
154 | /** Find columns with minimum d[j] and put them on the SCAN list.
155 | */
156 | uint_t _find_sparse_1(const uint_t n, uint_t lo, cost_t *d, int_t *cols, int_t *y)
157 | {
158 | uint_t hi = lo + 1;
159 | cost_t mind = d[cols[lo]];
160 | for (uint_t k = hi; k < n; k++) {
161 | int_t j = cols[k];
162 | if (d[j] <= mind) {
163 | if (d[j] < mind) {
164 | hi = lo;
165 | mind = d[j];
166 | }
167 | cols[k] = cols[hi];
168 | cols[hi++] = j;
169 | }
170 | }
171 | return hi;
172 | }
173 |
174 |
175 | /** Find columns with minimum d[j] and put them on the SCAN list.
176 | */
177 | int_t _find_sparse_2(cost_t *d, int_t *scan, const uint_t n_todo, int_t *todo, boolean *done)
178 | {
179 | int_t hi = 0;
180 | cost_t mind = LARGE;
181 | for (uint_t k = 0; k < n_todo; k++) {
182 | int_t j = todo[k];
183 | if (done[j]) {
184 | continue;
185 | }
186 | if (d[j] <= mind) {
187 | if (d[j] < mind) {
188 | hi = 0;
189 | mind = d[j];
190 | }
191 | scan[hi++] = j;
192 | }
193 | }
194 | return hi;
195 | }
196 |
197 |
198 | /** Scan all columns in TODO starting from arbitrary column in SCAN and try to
199 | * decrease d of the TODO columns using the SCAN column.
200 | */
201 | int_t _scan_sparse_1(
202 | const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
203 | uint_t *plo, uint_t *phi,
204 | cost_t *d, int_t *cols, int_t *pred,
205 | int_t *y, cost_t *v)
206 | {
207 | uint_t lo = *plo;
208 | uint_t hi = *phi;
209 | cost_t h, cred_ij;
210 |
211 | int_t *rev_kk;
212 | NEW(rev_kk, int_t, n);
213 |
214 | while (lo != hi) {
215 | int_t kj;
216 | int_t j = cols[lo++];
217 | const int_t i = y[j];
218 | const cost_t mind = d[j];
219 | for (uint_t k = 0; k < n; k++) {
220 | rev_kk[k] = -1;
221 | }
222 | for (uint_t k = ii[i]; k < ii[i+1]; k++) {
223 | const int_t j = kk[k];
224 | rev_kk[j] = k;
225 | }
226 | PRINTF("?%d kk[%d:%d]=", j, ii[i], ii[i+1]);
227 | PRINT_INDEX_ARRAY(kk + ii[i], ii[i+1] - ii[i]);
228 | kj = rev_kk[j];
229 | if (kj == -1) {
230 | continue;
231 | }
232 | ASSERT(kk[kj] == j);
233 | h = cc[kj] - v[j] - mind;
234 | PRINTF("i=%d j=%d kj=%d h=%f\n", i, j, kj, h);
235 | // For all columns in TODO
236 | for (uint_t k = hi; k < n; k++) {
237 | j = cols[k];
238 | PRINTF("?%d kk[%d:%d]=", j, ii[i], ii[i+1]);
239 | PRINT_INDEX_ARRAY(kk + ii[i], ii[i+1] - ii[i]);
240 | if ((kj = rev_kk[j]) == -1) {
241 | continue;
242 | }
243 | ASSERT(kk[kj] == j);
244 | cred_ij = cc[kj] - v[j] - h;
245 | if (cred_ij < d[j]) {
246 | d[j] = cred_ij;
247 | pred[j] = i;
248 | if (cred_ij == mind) {
249 | if (y[j] < 0) {
250 | FREE(rev_kk);
251 | return j;
252 | }
253 | cols[k] = cols[hi];
254 | cols[hi++] = j;
255 | }
256 | }
257 | }
258 | }
259 | *plo = lo;
260 | *phi = hi;
261 | FREE(rev_kk);
262 | return -1;
263 | }
264 |
265 |
266 | /** Scan all columns in TODO starting from arbitrary column in SCAN and try to
267 | * decrease d of the TODO columns using the SCAN column.
268 | */
269 | int_t _scan_sparse_2(
270 | const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
271 | uint_t *plo, uint_t *phi,
272 | cost_t *d, int_t *pred,
273 | boolean *done, uint_t *pn_ready, int_t *ready, int_t *scan,
274 | uint_t *pn_todo, int_t *todo, boolean *added,
275 | int_t *y, cost_t *v)
276 | {
277 | uint_t lo = *plo;
278 | uint_t hi = *phi;
279 | uint_t n_todo = *pn_todo;
280 | uint_t n_ready = *pn_ready;
281 | cost_t h, cred_ij;
282 |
283 | int_t *rev_kk;
284 | NEW(rev_kk, int_t, n);
285 |
286 | for (uint_t k = 0; k < n; k++) {
287 | rev_kk[k] = -1;
288 | }
289 | while (lo != hi) {
290 | int_t kj;
291 | int_t j = scan[lo++];
292 | const int_t i = y[j];
293 | ready[n_ready++] = j;
294 | const cost_t mind = d[j];
295 | for (uint_t k = ii[i]; k < ii[i+1]; k++) {
296 | const int_t j = kk[k];
297 | rev_kk[j] = k;
298 | }
299 | PRINTF("?%d kk[%d:%d]=", j, ii[i], ii[i+1]);
300 | PRINT_INDEX_ARRAY(kk + ii[i], ii[i+1] - ii[i]);
301 | kj = rev_kk[j];
302 | ASSERT(kj != -1);
303 | ASSERT(kk[kj] == j);
304 | h = cc[kj] - v[j] - mind;
305 | PRINTF("i=%d j=%d kj=%d h=%f\n", i, j, kj, h);
306 | // For all columns in TODO
307 | for (uint_t k = 0; k < ii[i+1] - ii[i]; k++) {
308 | j = kk[ii[i] + k];
309 | if (done[j]) {
310 | continue;
311 | }
312 | PRINTF("?%d kk[%d:%d]=", j, ii[i], ii[i+1]);
313 | PRINT_INDEX_ARRAY(kk + ii[i], ii[i+1] - ii[i]);
314 | cred_ij = cc[ii[i] + k] - v[j] - h;
315 | if (cred_ij < d[j]) {
316 | d[j] = cred_ij;
317 | pred[j] = i;
318 | if (cred_ij <= mind) {
319 | if (y[j] < 0) {
320 | FREE(rev_kk);
321 | return j;
322 | }
323 | scan[hi++] = j;
324 | done[j] = TRUE;
325 | } else if (!added[j]) {
326 | todo[n_todo++] = j;
327 | added[j] = TRUE;
328 | }
329 | }
330 | }
331 | for (uint_t k = ii[i]; k < ii[i+1]; k++) {
332 | const int_t j = kk[k];
333 | rev_kk[j] = -1;
334 | }
335 | }
336 | *pn_todo = n_todo;
337 | *pn_ready = n_ready;
338 | *plo = lo;
339 | *phi = hi;
340 | FREE(rev_kk);
341 | return -1;
342 | }
343 |
344 |
345 | /** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper.
346 | *
347 | * This version loops over all column indices (some of which might be inf).
348 | *
349 | * \return The closest free column index.
350 | */
351 | int_t find_path_sparse_1(
352 | const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
353 | const int_t start_i,
354 | int_t *y, cost_t *v,
355 | int_t *pred)
356 | {
357 | uint_t lo = 0, hi = 0;
358 | int_t final_j = -1;
359 | uint_t n_ready = 0;
360 | int_t *cols;
361 | cost_t *d;
362 |
363 | NEW(cols, int_t, n);
364 | NEW(d, cost_t, n);
365 |
366 | for (uint_t i = 0; i < n; i++) {
367 | cols[i] = i;
368 | d[i] = LARGE;
369 | pred[i] = start_i;
370 | }
371 | for (uint_t i = ii[start_i]; i < ii[start_i + 1]; i++) {
372 | const int_t j = kk[i];
373 | d[j] = cc[i] - v[j];
374 | }
375 | PRINT_COST_ARRAY(d, n);
376 | while (final_j == -1) {
377 | // No columns left on the SCAN list.
378 | if (lo == hi) {
379 | PRINTF("%d..%d -> find\n", lo, hi);
380 | n_ready = lo;
381 | hi = _find_sparse_1(n, lo, d, cols, y);
382 | PRINTF("check %d..%d\n", lo, hi);
383 | PRINT_INDEX_ARRAY(cols, n);
384 | for (uint_t k = lo; k < hi; k++) {
385 | const int_t j = cols[k];
386 | if (y[j] < 0) {
387 | final_j = j;
388 | }
389 | }
390 | }
391 | if (final_j == -1) {
392 | PRINTF("%d..%d -> scan\n", lo, hi);
393 | final_j = _scan_sparse_1(
394 | n, cc, ii, kk, &lo, &hi, d, cols, pred, y, v);
395 | PRINT_COST_ARRAY(d, n);
396 | PRINT_INDEX_ARRAY(cols, n);
397 | PRINT_INDEX_ARRAY(pred, n);
398 | }
399 | }
400 |
401 | PRINTF("found final_j=%d\n", final_j);
402 | PRINT_INDEX_ARRAY(cols, n);
403 | {
404 | const cost_t mind = d[cols[lo]];
405 | for (uint_t k = 0; k < n_ready; k++) {
406 | const int_t j = cols[k];
407 | v[j] += d[j] - mind;
408 | }
409 | }
410 |
411 | FREE(cols);
412 | FREE(d);
413 |
414 | return final_j;
415 | }
416 |
417 |
418 | /** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper.
419 | *
420 | * This version loops over non-inf column indices (which requires some additional bookkeeping).
421 | *
422 | * \return The closest free column index.
423 | */
424 | int_t find_path_sparse_2(
425 | const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
426 | const int_t start_i,
427 | int_t *y, cost_t *v,
428 | int_t *pred)
429 | {
430 | uint_t lo = 0, hi = 0;
431 | int_t final_j = -1;
432 | uint_t n_ready = 0;
433 | uint_t n_todo = (ii[start_i + 1] - ii[start_i]);
434 | boolean *done, *added;
435 | int_t *ready, *scan, *todo;
436 | cost_t *d;
437 |
438 | NEW(done, boolean, n);
439 | NEW(added, boolean, n);
440 | NEW(ready, int_t, n);
441 | NEW(scan, int_t, n);
442 | NEW(todo, int_t, n);
443 | NEW(d, cost_t, n);
444 |
445 | memset(done, FALSE, n);
446 | memset(added, FALSE, n);
447 | for (uint_t i = 0; i < n; i++) {
448 | d[i] = LARGE;
449 | pred[i] = start_i;
450 | }
451 | for (uint_t i = ii[start_i]; i < ii[start_i + 1]; i++) {
452 | const int_t j = kk[i];
453 | d[j] = cc[i] - v[j];
454 | todo[i - ii[start_i]] = j;
455 | added[j] = TRUE;
456 | }
457 | PRINT_COST_ARRAY(d, n);
458 | PRINT_INDEX_ARRAY(pred, n);
459 | PRINT_INDEX_ARRAY(done, n);
460 | PRINT_INDEX_ARRAY(ready, n_ready);
461 | PRINT_INDEX_ARRAY(scan + lo, hi - lo);
462 | PRINT_INDEX_ARRAY(todo, n_todo);
463 | PRINT_INDEX_ARRAY(added, n);
464 | while (final_j == -1) {
465 | // No columns left on the SCAN list.
466 | if (lo == hi) {
467 | PRINTF("%d..%d -> find\n", lo, hi);
468 | lo = 0;
469 | hi = _find_sparse_2(d, scan, n_todo, todo, done);
470 | PRINTF("check %d..%d\n", lo, hi);
471 | if (!hi) {
472 | // XXX: the assignment is unsolvable, lets try to return
473 | // something reasonable nevertheless.
474 | for (uint_t j = 0; j < n; j++) {
475 | if (!done[j] && y[j] < 0) {
476 | final_j = j;
477 | }
478 | }
479 | ASSERT(final_j != -1);
480 | break;
481 | }
482 | ASSERT(hi > lo);
483 | for (uint_t k = lo; k < hi; k++) {
484 | const int_t j = scan[k];
485 | if (y[j] < 0) {
486 | final_j = j;
487 | } else {
488 | done[j] = TRUE;
489 | }
490 | }
491 | }
492 | if (final_j == -1) {
493 | PRINTF("%d..%d -> scan\n", lo, hi);
494 | PRINT_INDEX_ARRAY(done, n);
495 | PRINT_INDEX_ARRAY(ready, n_ready);
496 | PRINT_INDEX_ARRAY(scan + lo, hi - lo);
497 | PRINT_INDEX_ARRAY(todo, n_todo);
498 | final_j = _scan_sparse_2(
499 | n, cc, ii, kk, &lo, &hi, d, pred,
500 | done, &n_ready, ready, scan,
501 | &n_todo, todo, added,
502 | y, v);
503 | PRINT_COST_ARRAY(d, n);
504 | PRINT_INDEX_ARRAY(pred, n);
505 | PRINT_INDEX_ARRAY(done, n);
506 | PRINT_INDEX_ARRAY(ready, n_ready);
507 | PRINT_INDEX_ARRAY(scan + lo, hi - lo);
508 | PRINT_INDEX_ARRAY(todo, n_todo);
509 | PRINT_INDEX_ARRAY(added, n);
510 | }
511 | }
512 |
513 | PRINTF("found final_j=%d\n", final_j);
514 | {
515 | const cost_t mind = d[scan[lo]];
516 | for (uint_t k = 0; k < n_ready; k++) {
517 | const int_t j = ready[k];
518 | v[j] += d[j] - mind;
519 | }
520 | }
521 |
522 | FREE(done);
523 | FREE(added);
524 | FREE(ready);
525 | FREE(scan);
526 | FREE(todo);
527 | FREE(d);
528 |
529 | return final_j;
530 | }
531 |
532 |
533 | /** Find path using one of the two find_path variants selected based on sparsity.
534 | */
535 | int_t find_path_sparse_dynamic(
536 | const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
537 | const int_t start_i,
538 | int_t *y, cost_t *v,
539 | int_t *pred)
540 | {
541 | const uint_t n_i = ii[start_i+1] - ii[start_i];
542 | // XXX: wouldnt it be better to decide for the whole matrix?
543 | if (n_i > 0.25 * n) {
544 | return find_path_sparse_1(n, cc, ii, kk, start_i, y, v, pred);
545 | } else {
546 | return find_path_sparse_2(n, cc, ii, kk, start_i, y, v, pred);
547 | }
548 | }
549 |
550 |
551 | typedef int_t (*fp_function_t)(
552 | const uint_t, cost_t *, uint_t *, uint_t *, const int_t, int_t *, cost_t *, int_t *);
553 |
554 | fp_function_t get_better_find_path(const uint_t n, uint_t *ii)
555 | {
556 | const double sparsity = ii[n] / (double)(n * n);
557 | if (sparsity > 0.25) {
558 | PRINTF("Using find_path_sparse_1 for sparsity=%f\n", sparsity);
559 | return find_path_sparse_1;
560 | } else {
561 | PRINTF("Using find_path_sparse_2 for sparsity=%f\n", sparsity);
562 | return find_path_sparse_2;
563 | }
564 | }
565 |
566 |
567 | /** Augment for a sparse cost matrix.
568 | */
569 | int_t _ca_sparse(
570 | const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
571 | const uint_t n_free_rows,
572 | int_t *free_rows, int_t *x, int_t *y, cost_t *v,
573 | int fp_version)
574 | {
575 | int_t *pred;
576 |
577 | NEW(pred, int_t, n);
578 |
579 | fp_function_t fp;
580 | switch (fp_version) {
581 | case FP_1: fp = find_path_sparse_1; break;
582 | case FP_2: fp = find_path_sparse_2; break;
583 | case FP_DYNAMIC: fp = get_better_find_path(n, ii); break;
584 | default: return -2;
585 | }
586 |
587 | for (int_t *pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) {
588 | int_t i = -1, j;
589 | uint_t k = 0;
590 |
591 | PRINTF("looking at free_i=%d\n", *pfree_i);
592 | j = fp(n, cc, ii, kk, *pfree_i, y, v, pred);
593 | ASSERT(j >= 0);
594 | ASSERT(j < n);
595 | while (i != *pfree_i) {
596 | PRINTF("augment %d\n", j);
597 | PRINT_INDEX_ARRAY(pred, n);
598 | i = pred[j];
599 | PRINTF("y[%d]=%d -> %d\n", j, y[j], i);
600 | y[j] = i;
601 | PRINT_INDEX_ARRAY(x, n);
602 | SWAP_INDICES(j, x[i]);
603 | k++;
604 | if (k >= n) {
605 | ASSERT(FALSE);
606 | }
607 | }
608 | }
609 | FREE(pred);
610 | return 0;
611 | }
612 |
613 |
614 | /** Solve square sparse LAP.
615 | */
616 | int lapmod_internal(
617 | const uint_t n, cost_t *cc, uint_t *ii, uint_t *kk,
618 | int_t *x, int_t *y, fp_t fp_version)
619 | {
620 | int ret;
621 | int_t *free_rows;
622 | cost_t *v;
623 |
624 | NEW(free_rows, int_t, n);
625 | NEW(v, cost_t, n);
626 | ret = _ccrrt_sparse(n, cc, ii, kk, free_rows, x, y, v);
627 | int i = 0;
628 | while (ret > 0 && i < 2) {
629 | ret = _carr_sparse(n, cc, ii, kk, ret, free_rows, x, y, v);
630 | i++;
631 | }
632 | if (ret > 0) {
633 | ret = _ca_sparse(n, cc, ii, kk, ret, free_rows, x, y, v, fp_version);
634 | }
635 | FREE(v);
636 | FREE(free_rows);
637 | return ret;
638 | }
639 |
--------------------------------------------------------------------------------
/include/image_utils/tracker.h:
--------------------------------------------------------------------------------
1 | #ifndef TRACKER_H
2 | #define TRACKER_H
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include "image_utils/detect_process.h"
10 | #include "print_utils.h"
11 | #include "image_utils/lap/lapjv.h"
12 |
13 |
14 | namespace LAP {
15 |
16 | void lapjv(const Eigen::MatrixXd& costMatrix, double costLimit,
17 | std::vector& x, std::vector& y) {
18 | //
19 | int n_rows = costMatrix.rows();
20 | int n_cols = costMatrix.cols();
21 | x.clear();
22 | y.clear();
23 | x.resize(n_rows);
24 | y.resize(n_cols);
25 | const int n = n_rows + n_cols;
26 |
27 | costLimit /= 2.;
28 |
29 | double **cost_ptr = new double*[n];
30 | for (int i=0; i=n_rows && j>=n_cols) {
37 | cost_ptr[i][j] = 0.;
38 | }
39 | else {
40 | cost_ptr[i][j] = costLimit;
41 | }
42 | }
43 | }
44 |
45 | int_t x_c[n], y_c[n];
46 | lapjv_internal(n, cost_ptr, &x_c[0], &y_c[0]);
47 |
48 | for (int i=0; i= n_cols)?-1:x_c[i];
53 | }
54 |
55 | for (int i=0;i= n_rows)?-1:y_c[i];
57 | }
58 |
59 | }
60 |
61 |
62 | void linearAssignment(const Eigen::MatrixXd& costMatrix, double thresh,
63 | std::vector>& matches,
64 | std::vector& unmatched_a,
65 | std::vector& unmatched_b) {
66 |
67 | // INFO << "cost matrix:(" << costMatrix.cols() << "," << costMatrix.rows() << ")"
68 | // << "\n" << costMatrix << "\n" << ENDL;
69 |
70 |
71 | matches.clear();
72 | unmatched_a.clear();
73 | unmatched_b.clear();
74 |
75 | int n = costMatrix.rows();
76 | int m = costMatrix.cols();
77 |
78 | if (n == 0 || m == 0) {
79 | for (int i=0;i x, y;
85 |
86 | lapjv(costMatrix, thresh, x, y);
87 |
88 | for (int i = 0; i < n; ++i) {
89 | if (x[i] < 0) {
90 | unmatched_a.push_back(i);
91 | }
92 | else {
93 | std::vector this_match;
94 | this_match.push_back(i);
95 | this_match.push_back(x[i]);
96 | matches.push_back(this_match);
97 | }
98 | }
99 | for (int j = 0; j < m; ++j) {
100 | if (y[j] < 0) {
101 | unmatched_b.push_back(j);
102 | }
103 | }
104 | }
105 | }
106 |
107 |
108 |
109 | enum struct TrackStatus { Tentative, Confirmed, Coasted, Deleted };
110 |
111 | namespace Kalman {
112 |
113 | class Filter {
114 | public:
115 | Filter(int dim_x, int dim_z, int dim_u = 0)
116 | : dim_x(dim_x), dim_z(dim_z), dim_u(dim_u),
117 | x(dim_x, 1), P(dim_x, dim_x), Q(dim_x, dim_x),
118 | F(dim_x, dim_x), H(dim_z, dim_x), R(dim_z, dim_z),
119 | _alpha_sq(1.0), M(dim_z, dim_z), z(dim_z, 1),
120 | K(dim_x, dim_z), y(dim_z, 1), S(dim_z, dim_z),
121 | SI(dim_z, dim_z), _I(dim_x, dim_x),
122 | x_prior(dim_x, 1), P_prior(dim_x, dim_x),
123 | x_post(dim_x, 1), P_post(dim_x, dim_x) {
124 | x.setZero();
125 | P.setIdentity();
126 | Q.setIdentity();
127 | F.setIdentity();
128 | H.setZero();
129 | R.setIdentity();
130 | M.setZero();
131 | z.setOnes() * std::numeric_limits::quiet_NaN();
132 | K.setZero();
133 | y.setZero();
134 | S.setZero();
135 | SI.setZero();
136 | _I.setIdentity();
137 | x_prior = x;
138 | P_prior = P;
139 | x_post = x;
140 | P_post = P;
141 | }
142 |
143 | void predict() {
144 | x = F * x;
145 | P = _alpha_sq * (F * P * F.transpose()) + Q;
146 | x_prior = x;
147 | P_prior = P;
148 | }
149 |
150 | void update(const Eigen::VectorXd& z,
151 | const Eigen::MatrixXd& R) {
152 | if (R.rows() != dim_z || R.cols() != dim_z) {
153 | throw std::invalid_argument("R matrix has incorrect dimensions");
154 | }
155 |
156 | // Update state
157 | y = z - H * x;
158 | Eigen::MatrixXd PHT = P_prior * H.transpose();
159 | S = H * PHT + R;
160 | SI = S.inverse();
161 | K = PHT * S.inverse();
162 | x += K * y;
163 |
164 | // Update covariance
165 | Eigen::MatrixXd I_KH = _I - K * H;
166 | P = (I_KH * P_prior) * I_KH.transpose() + (K * R) * K.transpose();
167 |
168 | // Prepare for next prediction
169 | this->z = z;
170 | x_post = x;
171 | P_post = P;
172 |
173 | }
174 |
175 | Eigen::VectorXd getState() const {
176 | return x;
177 | }
178 |
179 | Eigen::MatrixXd getCovariance() const {
180 | return P;
181 | }
182 |
183 | // private:
184 |
185 | int dim_x, dim_z, dim_u;
186 | Eigen::VectorXd x, x_prior, x_post;
187 | Eigen::MatrixXd F, P, Q, H, R, M, z, K, y, S, SI, _I, P_prior, P_post;
188 | double _alpha_sq;
189 | };
190 |
191 |
192 |
193 | class Tracker {
194 | public:
195 | Tracker(Eigen::Vector2d y, Eigen::Matrix2d R, double wx, double wy, double vmax,
196 | double w, double h, double dt = 1.0 / 30.0, int tracker_count = 0);
197 | void update(const Eigen::Vector2d& y, Eigen::Matrix2d& R);
198 | Eigen::Vector2d predict();
199 | Eigen::Vector4d getState() const ;
200 | double distance(const Eigen::Vector2d& y, Eigen::Matrix2d& R, bool show) const ;
201 |
202 | // 其他成员变量
203 | int id;
204 | int age;
205 | int death_count;
206 | int birth_count;
207 | int detidx;
208 | double w;
209 | double h;
210 | TrackStatus status;
211 |
212 | private:
213 | Filter kf;
214 | };
215 |
216 |
217 | Tracker::Tracker(Eigen::Vector2d y, Eigen::Matrix2d R, double wx, double wy,
218 | double vmax, double w, double h, double dt, int tracker_count)
219 | : id(tracker_count), age(0), death_count(0), birth_count(0), detidx(-1),
220 | w(w), h(h), status(TrackStatus::Tentative), kf(4, 2) {
221 | kf.F = Eigen::Matrix4d::Identity();
222 | kf.F(0, 1) = dt;
223 | kf.F(2, 3) = dt;
224 | kf.H = Eigen::Matrix::Zero();
225 | kf.H(0, 0) = 1;
226 | kf.H(1, 2) = 1;
227 | kf.R = R;
228 | kf.P.setZero(); //= Eigen::Matrix::Zero();
229 | kf.P << 1, 0, 0, 0,
230 | 0, vmax * vmax / 3.0, 0, 0,
231 | 0, 0, 1, 0,
232 | 0, 0, 0, vmax * vmax / 3.0;
233 |
234 | // INFO << "P:\n" << kf.P << ENDL;
235 |
236 | Eigen::Matrix G;
237 | G << 0.5 * dt * dt, 0,
238 | dt, 0,
239 | 0, 0.5 * dt * dt,
240 | 0, dt;
241 | Eigen::Matrix Q0;
242 | Q0.setZero();
243 | Q0(0, 0) = wx;
244 | Q0(1, 1) = wy;
245 |
246 | Eigen::MatrixXd _Q = (G * Q0) * G.transpose();
247 | kf.Q = _Q;
248 |
249 | kf.x << y(0), 0, y(1), 0;
250 | }
251 |
252 | void Tracker::update(const Eigen::Vector2d& y, Eigen::Matrix2d& R) {
253 | kf.update(y, R);
254 | }
255 |
256 | Eigen::Vector2d Tracker::predict() {
257 | kf.predict();
258 | this->age += 1;
259 | return kf.H * kf.x;
260 | }
261 |
262 | Eigen::Vector4d Tracker::getState() const {
263 | return kf.x;
264 | }
265 |
266 | double Tracker::distance(const Eigen::Vector2d& y, Eigen::Matrix2d& R, bool show=false) const {
267 | Eigen::Vector2d diff = y - (kf.H * kf.x);
268 | Eigen::Matrix2d S = kf.H * (kf.P * kf.H.transpose()) + R;
269 | Eigen::Matrix2d SI = S.inverse();
270 | auto mahalanobis = diff.transpose() * (SI * diff);
271 | double logdet = log(S.determinant());
272 |
273 | if (show) {
274 | // debug
275 | INFO << "y:\n" << y << ENDL;
276 | INFO << "R:\n" << R << ENDL;
277 |
278 | INFO << "w:\n" << w << ENDL;
279 | INFO << "h:\n" << h << ENDL;
280 | INFO << "kf.H:\n" << kf.H << ENDL;
281 | INFO << "kf.P:\n" << kf.P << ENDL;
282 | INFO << "kf.x:\n" << kf.x << ENDL;
283 |
284 | INFO << "diff:\n" << diff << ENDL;
285 | INFO << "S:\n" << S << ENDL;
286 | INFO << "SI:\n" << SI << ENDL;
287 | INFO << "mahalanobis:\n" << mahalanobis << ENDL;
288 | INFO << "logdet:\n" << logdet << ENDL;
289 | }
290 |
291 | return mahalanobis(0, 0) + logdet;
292 | }
293 | };
294 |
295 |
296 | namespace UCMC {
297 |
298 | struct Obj {
299 | int id=0;
300 | int track_idx=0;
301 | detect::Object obj;
302 | Eigen::Matrix y;
303 | Eigen::Matrix2d R;
304 | };
305 |
306 | class Mapper {
307 |
308 | Eigen::Matrix3d A, InvA;
309 | Eigen::MatrixXd KiKo;
310 |
311 | public:
312 |
313 | bool debug=false;
314 |
315 | Mapper() {};
316 |
317 | Mapper(std::vector& _Ki, std::vector& _Ko) {
318 | Eigen::Map> KiT(_Ki.data());
319 | Eigen::Map KoT(_Ko.data());
320 |
321 | Eigen::Matrix Ki = KiT.transpose();
322 | Eigen::Matrix4d Ko = KoT.transpose();
323 |
324 | A.setZero();
325 | InvA.setZero();
326 | KiKo = Ki * Ko;
327 |
328 | for (int now_row=0;now_row<3;now_row++)
329 | for (int now_col=0;now_col<3;now_col++) {
330 | A(now_col, now_row) = KiKo(now_col, now_row);
331 | if (now_row==2) {
332 | A(now_col, now_row) = KiKo(now_col, 3);
333 | }
334 | }
335 |
336 | InvA = A.inverse();
337 |
338 | if (debug) {
339 | INFO << "A: " << A << ENDL;
340 | INFO << "InvA: " << InvA << ENDL;
341 | INFO << "Ki: " << Ki << ENDL;
342 | INFO << "Ko: " << Ko << ENDL;
343 | INFO << "KiKo: " << KiKo << ENDL;
344 | }
345 | }
346 |
347 | std::vector uvError(cv::Rect box) {
348 | std::vector uv;
349 | uv.resize(2);
350 | uv[0] = MAX(MIN(13., 0.05 * box.width), 2.);
351 | uv[1] = MAX(MIN(10., 0.05 * box.height), 2.);
352 | return uv;
353 | }
354 |
355 | void uv2xy(Eigen::MatrixXd uv, Eigen::MatrixXd sigma_uv,
356 | Eigen::Matrix& xy, Eigen::Matrix2d& sigma_xy) {
357 | Eigen::Matrix uv1;
358 | uv1(0, 0) = uv(0, 0);
359 | uv1(1, 0) = uv(1, 0);
360 | uv1(2, 0) = 1.;
361 |
362 |
363 | Eigen::MatrixXd b = InvA * uv1;
364 |
365 | // INFO << "A " << A << ENDL;
366 | // INFO << "InvA:\n" << InvA << ENDL;
367 | // INFO << "uv1:\n" << uv1 << ENDL;
368 | // INFO << "b:\n" << b << ENDL;
369 |
370 |
371 | double gamma = 1. / b(2, 0);
372 |
373 | Eigen::Matrix2d C = gamma * InvA.block(0, 0, 2, 2)
374 | - (gamma * gamma) * b.block(0, 0, 2, 1) * InvA.block(2, 0, 1, 2);
375 |
376 | // INFO << "C:\n" << C << ENDL;
377 | // INFO << "sigma_uv:\n" << sigma_uv << ENDL;
378 |
379 | xy = b.block(0, 0, 2, 1) * gamma;
380 | sigma_xy = (C * sigma_uv) * C.transpose();
381 |
382 | }
383 |
384 | // void xy2uv(double x, double y, double& u, double& v);
385 |
386 | void map_to(cv::Rect box, Eigen::Matrix& y, Eigen::Matrix2d& R){
387 | Eigen::Matrix uv;
388 | uv(0, 0) = box.x + 0.5 * box.width;
389 | uv(1, 0) = box.y + box.height;
390 | std::vector uv_err = uvError(box);
391 | Eigen::MatrixXd sigma_uv = Eigen::Matrix2d::Identity();
392 | sigma_uv(0, 0) = uv_err[1] * uv_err[1];
393 | sigma_uv(1, 1) = uv_err[1] * uv_err[1];
394 | uv2xy(uv, sigma_uv, y, R);
395 |
396 |
397 | // INFO << "y: " << y << ENDL;
398 | // INFO << "R: " << R << ENDL;
399 | }
400 |
401 | };
402 |
403 | struct Params {
404 | double a1 = 100.;
405 | double a2 = 100.;
406 | double wx = 5.;
407 | double wy = 5.;
408 | double vmax = 10.;
409 | double max_age = 10.;
410 | double high_score = 0.5;
411 | double conf_threshold = 0.01;
412 | double dt = 0.033;
413 | std::string dataset = "MOT";
414 | std::vector Ki;
415 | std::vector Ko;
416 | };
417 |
418 | class Tracker
419 | {
420 | private:
421 | Params params;
422 | std::vector trackers;
423 | int frame_idx=0;
424 | int tracker_count = 0;
425 | std::vector confirmed_idx, coasted_idx, tentative_idx, detidx_remain;
426 | public:
427 |
428 | Mapper mapper;
429 | bool debug=false;
430 |
431 | Tracker(Params& params);
432 |
433 | std::vector update(std::vector &det_results) {
434 | std::vector dets;
435 | int id=0;
436 | for (detect::Object obj: det_results) {
437 | Obj this_obj;
438 | this_obj.id = id++;
439 | this_obj.obj = obj;
440 | mapper.map_to(obj.rect, this_obj.y, this_obj.R);
441 |
442 | dets.push_back(this_obj);
443 | }
444 | this->update(dets, frame_idx++);
445 | return dets;
446 | }
447 |
448 | void update(std::vector &dets, int frame_id);
449 |
450 | void data_association(std::vector &dets, int frame_id);
451 |
452 | void associate_tentative(std::vector &dets);
453 |
454 | void initial_tentative(std::vector &dets);
455 |
456 | void delete_old_trackers();
457 |
458 | void update_status(std::vector &dets);
459 |
460 | ~Tracker();
461 | };
462 |
463 | Tracker::Tracker(Params& params)
464 | {
465 | this->params = params;
466 | mapper = Mapper(params.Ki, params.Ko);
467 | }
468 |
469 | void Tracker::update(std::vector &dets, int frame_id) {
470 | this->data_association(dets, frame_id);
471 | this->associate_tentative(dets);
472 | this->initial_tentative(dets);
473 | this->delete_old_trackers();
474 | this->update_status(dets);
475 | }
476 |
477 | void Tracker::data_association(std::vector &dets, int frame_id) {
478 | std::vector detidx_high, detidx_low;
479 | for (size_t i = 0; i < dets.size(); ++i) {
480 | if (dets[i].obj.prob >= params.high_score) {
481 | detidx_high.push_back(i);
482 | } else {
483 | detidx_low.push_back(i);
484 | }
485 | }
486 |
487 | for (auto& track : trackers) {
488 | track.predict();
489 | }
490 |
491 | std::vector trackidx_remain;
492 | detidx_remain.clear();
493 |
494 | // 关联高分检测与轨迹
495 | // INFO << "try high score mapping" << ENDL;
496 | std::vector trackidx = confirmed_idx;
497 | trackidx.insert(trackidx.end(), coasted_idx.begin(), coasted_idx.end());
498 |
499 | int num_det = detidx_high.size();
500 | int num_trk = trackidx.size();
501 |
502 | // 初始化轨迹的detidx为-1
503 | for (auto& track : trackers) {
504 | track.detidx = -1;
505 | }
506 |
507 | if (num_det * num_trk > 0) {
508 | Eigen::MatrixXd cost_matrix(num_det, num_trk);
509 | cost_matrix.setZero();
510 |
511 | for (int i = 0; i < num_det; ++i) {
512 | int det_idx = detidx_high[i];
513 | for (int j = 0; j < num_trk; ++j) {
514 | cost_matrix(i, j) = trackers[trackidx[j]].distance(
515 | dets[det_idx].y,
516 | dets[det_idx].R,
517 | i+j==0 && debug
518 | );
519 | }
520 | }
521 |
522 | // INFO << "det[0].y:\n" << dets[0].y << ENDL;
523 | // INFO << "det[0].R:\n" << dets[0].R << ENDL;
524 |
525 |
526 | // INFO << "cost matrix: " << cost_matrix << ENDL;
527 |
528 | std::vector> matched_indices;
529 | std::vector unmatched_a;
530 | std::vector unmatched_b;
531 | LAP::linearAssignment(cost_matrix, this->params.a1,
532 | matched_indices, unmatched_a, unmatched_b);
533 |
534 | // INFO << "high: " << detidx_remain.size() << " " << unmatched_a.size() << " "
535 | // << unmatched_b.size() << ENDL;
536 |
537 | // 处理未匹配的检测和轨迹
538 | for (int i : unmatched_a) {
539 | detidx_remain.push_back(detidx_high[i]);
540 | }
541 | for (int i : unmatched_b) {
542 | trackidx_remain.push_back(trackidx[i]);
543 | }
544 |
545 | for (size_t i=0; i 0) {
570 | Eigen::MatrixXd cost_matrix(num_det_low, num_trk_remain);
571 | cost_matrix.setZero();
572 |
573 | for (int i = 0; i < num_det_low; ++i) {
574 | int det_idx = detidx_low[i];
575 | for (int j = 0; j < num_trk_remain; ++j) {
576 | int trk_idx = trackidx_remain[j];
577 | cost_matrix(i, j) = trackers[trk_idx].distance(
578 | dets[det_idx].y,
579 | dets[det_idx].R,
580 | i+j==0 && debug
581 | );
582 | }
583 | }
584 |
585 | // INFO << "det[0].y:\n" << dets[0].y << ENDL;
586 | // INFO << "det[0].R:\n" << dets[0].R << ENDL;
587 |
588 | std::vector> matched_indices;
589 | std::vector unmatched_a;
590 | std::vector unmatched_b;
591 | LAP::linearAssignment(cost_matrix, this->params.a2,
592 | matched_indices, unmatched_a, unmatched_b);
593 |
594 | // 处理未匹配的轨迹
595 | for (int i : unmatched_b) {
596 | int trk_idx = trackidx_remain[i];
597 | trackers[trk_idx].status = TrackStatus::Coasted;
598 | // trackers[trk_idx].death_count += 1; // 如果需要的话
599 | trackers[trk_idx].detidx = -1;
600 | }
601 |
602 | // 更新匹配的轨迹
603 | for (size_t i=0; i &dets) {
618 | // INFO << "try associate_tentative mapping" << ENDL;
619 | size_t num_det = detidx_remain.size();
620 | size_t num_trk = tentative_idx.size();
621 |
622 | Eigen::MatrixXd cost_matrix(num_det, num_trk);
623 | cost_matrix.setZero();
624 |
625 | for (int i=0;i> matched_indices;
641 | std::vector unmatched_a;
642 | std::vector unmatched_b;
643 | LAP::linearAssignment(cost_matrix, this->params.a1,
644 | matched_indices, unmatched_a, unmatched_b);
645 |
646 | // INFO << detidx_remain.size() << " " << unmatched_a.size() << " "
647 | // << unmatched_b.size() << ENDL;
648 |
649 | for (size_t i=0; i= 2) {
659 | trackers[trk_idx].birth_count = 0;
660 | trackers[trk_idx].status = TrackStatus::Confirmed;
661 | }
662 | }
663 |
664 | for (int i: unmatched_b) {
665 | int trk_idx = tentative_idx[i];
666 | trackers[trk_idx].detidx--;
667 | }
668 |
669 | std::vector unmatched_detidx;
670 | for (int i: unmatched_a) {
671 | unmatched_detidx.push_back(detidx_remain[i]);
672 | }
673 | detidx_remain = unmatched_detidx;
674 | }
675 |
676 | void Tracker::initial_tentative(std::vector &dets) {
677 | // INFO << detidx_remain.size() << ENDL;
678 | for (int i: detidx_remain) {
679 | // INFO << "detidx:" << i << "" << dets[i].obj.rect << ENDL;
680 | Kalman::Tracker new_obj(
681 | dets[i].y,
682 | dets[i].R,
683 | this->params.wx,
684 | this->params.wy,
685 | this->params.vmax,
686 | dets[i].obj.rect.width,
687 | dets[i].obj.rect.height,
688 | this->params.dt,
689 | ++this->tracker_count
690 | );
691 | new_obj.status = TrackStatus::Tentative;
692 | new_obj.detidx = i;
693 | trackers.push_back(new_obj);
694 | }
695 | detidx_remain.clear();
696 | }
697 |
698 | void Tracker::delete_old_trackers() {
699 | std::vector idx_reserve;
700 | std::vector reserve_trackers;
701 | for (int trk_idx=0; trk_idx= this->params.max_age) ||
705 | (trackers[trk_idx].status == TrackStatus::Tentative &&
706 | trackers[trk_idx].death_count >= 2))) {
707 | reserve_trackers.push_back(trackers.at(trk_idx));
708 | }
709 | }
710 | trackers = reserve_trackers;
711 | }
712 |
713 | void Tracker::update_status(std::vector &dets) {
714 | confirmed_idx.clear();
715 | coasted_idx.clear();
716 | tentative_idx.clear();
717 |
718 | for (int i=0;i= 0 && detidx < dets.size()) {
722 | trackers[i].h = dets[detidx].obj.rect.height;
723 | trackers[i].w = dets[detidx].obj.rect.width;
724 | }
725 |
726 | switch (trackers[i].status)
727 | {
728 | case TrackStatus::Confirmed:
729 | confirmed_idx.push_back(i);
730 | break;
731 | case TrackStatus::Coasted:
732 | coasted_idx.push_back(i);
733 | break;
734 | case TrackStatus::Tentative:
735 | tentative_idx.push_back(i);
736 | break;
737 | default:
738 | break;
739 | }
740 |
741 | }
742 | }
743 |
744 | Tracker::~Tracker() {
745 | trackers.clear();
746 | confirmed_idx.clear();
747 | coasted_idx.clear();
748 | tentative_idx.clear();
749 | detidx_remain.clear();
750 | }
751 |
752 | }
753 |
754 |
755 | namespace Track {
756 |
757 | static cv::Mat draw_boxes(cv::Mat image,
758 | std::vector &objects,
759 | std::vector& names,
760 | int draw_size=20,
761 | bool draw_label=true) {
762 | cv::Mat d_img = image.clone();
763 | cv::Scalar color;
764 | cv::Scalar txt_color;
765 | cv::Scalar txt_bk_color;
766 | cv::Size label_size;
767 | int baseLine = 0;
768 | int x, y, out_point_y, w, h;
769 | int line_thickness = std::round((double)draw_size / 10.0);
770 |
771 | for(int k=0; k 127)?cv::Scalar(0, 0, 0):cv::Scalar(255, 255, 255);
788 | std::string label = std::to_string(objects.at(k).track_idx) + " " + names.at(objects.at(k).obj.label) + " " + std::to_string(objects.at(k).obj.prob).substr(0, 4);
789 | label_size = cv::getTextSize(label.c_str(), cv::LINE_AA, double(draw_size) / 30.0, (line_thickness>1)?line_thickness-1:1, &baseLine);
790 | txt_bk_color = color; // * 0.7;
791 | y = (y > d_img.rows)?d_img.rows:y + 1;
792 | out_point_y = y - label_size.height - baseLine;
793 | if (out_point_y >= 0) y = out_point_y;
794 | cv::rectangle(d_img, cv::Rect(cv::Point(x - (line_thickness - 1), y), cv::Size(label_size.width, label_size.height + baseLine)),
795 | txt_bk_color, -1);
796 | cv::putText(d_img, label, cv::Point(x, y + label_size.height),
797 | cv::LINE_AA, double(draw_size) / 30.0, txt_color, (line_thickness>1)?line_thickness-1:1);
798 | }
799 |
800 | }
801 | return d_img;
802 | }
803 | }
804 |
805 |
806 | #endif
807 |
--------------------------------------------------------------------------------
/include/print_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef PRINT_UTILS_H
2 | #define PRINT_UTILS_H
3 |
4 | #define YELLOW "\033[33m" /* Yellow */
5 | #define GREEN "\033[32m" /* Green */
6 | #define RED "\033[31m" /* Red */
7 | #define END "\033[0m"
8 | #define ENDL "\033[0m" << std::endl
9 |
10 | #define WARN (std::cout << YELLOW)
11 | #define INFO (std::cout << GREEN)
12 | #define ERROR (std::cout << RED)
13 |
14 | #include
15 |
16 | static timeval get_now_time() {
17 | timeval _t;
18 | gettimeofday(&_t, NULL);
19 | return _t;
20 | }
21 |
22 | static int get_time_interval(timeval _t1, timeval _t2) {
23 | return (int)((_t2.tv_sec - _t1.tv_sec) * 1000 + (_t2.tv_usec - _t1.tv_usec) / 1000);
24 | }
25 |
26 | static float get_time_interval_f(timeval _t1, timeval _t2) {
27 | return ((float)(_t2.tv_sec - _t1.tv_sec) * 1000. + (float)(_t2.tv_usec - _t1.tv_usec) / 1000.);
28 | }
29 |
30 | class TimeCount {
31 | std::vector tics;
32 | std::vector tocs;
33 |
34 | public:
35 | size_t length() {
36 | return tics.size();
37 | }
38 |
39 | void tic(int idx) {
40 | timeval now_time = get_now_time();
41 | while (idx >= this->length()) {
42 | tics.push_back(now_time);
43 | tocs.push_back(now_time);
44 | }
45 | tics[idx] = now_time;
46 | tocs[idx] = now_time;
47 | }
48 |
49 | int get_timeval(int idx) {
50 | idx = MIN(idx, this->length()-1);
51 | return get_time_interval(tics[idx], tocs[idx]);
52 | }
53 |
54 | float get_timeval_f(int idx) {
55 | idx = MIN(idx, this->length()-1);
56 | return get_time_interval_f(tics[idx], tocs[idx]);
57 | }
58 |
59 | int toc(int idx) {
60 | idx = MIN(idx, this->length()-1);
61 | tocs[idx] = get_now_time();
62 | return this->get_timeval(idx);
63 | }
64 |
65 | };
66 |
67 | #endif
--------------------------------------------------------------------------------
/scripts/estimate_cam_param.py:
--------------------------------------------------------------------------------
1 | import argparse
2 | import cv2
3 | import numpy as np
4 | import copy
5 | import yaml
6 |
7 | ori_value = {
8 | 'theta_xy': 250,
9 | 'theta_z': 500,
10 | 'focal': 200,
11 | 'Tz': 200
12 | }
13 |
14 |
15 | class CameraPara:
16 |
17 | def open(self, file):
18 | self.data = yaml.load(open(file), yaml.SafeLoader)
19 | self.Ki = np.array(self.data["Ki"]).reshape([3, 4])
20 | self.Ko = np.array(self.data["Ko"]).reshape([4, 4])
21 |
22 | def xy2uv(self, x, y):
23 | uv = np.dot(self.Ki, np.dot(self.Ko, np.array([x,y,0,1])))
24 | uv /= uv[2]
25 | return int(uv[0]), int(uv[1])
26 |
27 |
28 | def xy2uv(x,y,Ki,Ko):
29 | # 计算uv
30 | uv = np.dot(Ki, np.dot(Ko, np.array([x,y,0,1])))
31 | # 归一化
32 | uv = uv/uv[2]
33 | return int(uv[0]), int(uv[1])
34 |
35 |
36 | # 定义转换函数
37 | def get_real_theta(value):
38 | return (value - ori_value["theta_xy"]) / 10.0
39 |
40 | def get_real_theta_z(value):
41 | return (value - ori_value["theta_z"]) / 5.0
42 |
43 | def get_real_focal(value):
44 | return (value - ori_value["focal"]) * 5
45 |
46 | def get_real_transition(value):
47 | return (value-ori_value["Tz"]) * 0.04
48 |
49 |
50 | # 定义滑动条回调函数
51 | def update_value_display():
52 | global value_display,g_theta_x,g_theta_y,g_theta_z,g_focal,g_tz
53 | value_display.fill(0) # 清空图像
54 | text = f"theta_x: {g_theta_x:.2f}"
55 | cv2.putText(value_display, text, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2)
56 | text = f"theta_y: {g_theta_y:.2f}"
57 | cv2.putText(value_display, text, (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2)
58 | text = f"theta_z: {g_theta_z:.2f}"
59 | cv2.putText(value_display, text, (10, 180), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2)
60 | text = f"focal: {g_focal}"
61 | cv2.putText(value_display, text, (10, 240), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2)
62 | text = f"Tz: {g_tz:.2f}"
63 | cv2.putText(value_display, text, (10, 300), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2)
64 | cv2.imshow('Values', value_display)
65 |
66 | def on_theta_x_change(value):
67 | global g_theta_x
68 | g_theta_x = get_real_theta(value)
69 | update_value_display()
70 |
71 | def on_theta_y_change(value):
72 | global g_theta_y
73 | g_theta_y = get_real_theta(value)
74 | update_value_display()
75 |
76 | def on_theta_z_change(value):
77 | global g_theta_z
78 | g_theta_z = get_real_theta_z(value)
79 | update_value_display()
80 |
81 | def on_focal_change(value):
82 | global g_focal
83 | g_focal = get_real_focal(value)
84 | update_value_display()
85 |
86 | def on_tz_change(value):
87 | global g_tz
88 | g_tz = get_real_transition(value)
89 | update_value_display()
90 |
91 | cv2.namedWindow('Values')
92 | # 初始化一个空白图像来显示实际值
93 | value_display = np.zeros((400, 300), dtype=np.uint8)
94 | g_theta_x = 0
95 | g_theta_y = 0
96 | g_theta_z = 0
97 | g_focal = 0
98 | g_tz = 0
99 |
100 | def main(args):
101 |
102 | cam_para = CameraPara()
103 | cam_para.open(args.cam_para)
104 |
105 | if args.img is not None:
106 | img = cv2.imread(args.img)
107 | else:
108 | cap = cv2.VideoCapture(args.vid)
109 | import os.path as osp
110 | cap.set(cv2.CAP_PROP_POS_FRAMES, args.id)
111 | success, img = cap.read()
112 | assert success and img is not None
113 | # 获取img的大小
114 | height, width = img.shape[:2]
115 | ori_img = img.copy()
116 |
117 | cv2.namedWindow('CamParaSettings')
118 | # 添加ui界面来修改theta_x,theta_y,theta_z, 调节访问是-10到10，间隔0.2
119 | cv2.createTrackbar('theta_x', 'CamParaSettings', ori_value["theta_xy"],500, on_theta_x_change)
120 | cv2.createTrackbar('theta_y', 'CamParaSettings', ori_value["theta_xy"],500, on_theta_y_change)
121 | cv2.createTrackbar('theta_z', 'CamParaSettings', ori_value["theta_z"],1000, on_theta_z_change)
122 | cv2.createTrackbar('focal', 'CamParaSettings', ori_value["focal"],500, on_focal_change)
123 | cv2.createTrackbar('Tz', 'CamParaSettings', ori_value["Tz"],500, on_tz_change)
124 |
125 | global g_theta_x,g_theta_y,g_theta_z
126 |
127 | # 循环一直到按下q or esc
128 | while True:
129 | theta_x = g_theta_x/180.0*np.pi
130 | theta_y = g_theta_y/180.0*np.pi
131 | theta_z = g_theta_z/180.0*np.pi
132 | Ki = copy.copy(cam_para.Ki)
133 | Ko = copy.copy(cam_para.Ko)
134 | R = Ko[0:3,0:3]
135 | Rx = np.array([[1,0,0],[0,np.cos(theta_x),-np.sin(theta_x)],[0,np.sin(theta_x),np.cos(theta_x)]])
136 | Ry = np.array([[np.cos(theta_y),0,np.sin(theta_y)],[0,1,0],[-np.sin(theta_y),0,np.cos(theta_y)]])
137 | Rz = np.array([[np.cos(theta_z),-np.sin(theta_z),0],[np.sin(theta_z),np.cos(theta_z),0],[0,0,1]])
138 | R = np.dot(R, np.dot(Rx, np.dot(Ry,Rz)))
139 | Ko[0:3,0:3] = R
140 | Ko[2,3] += g_tz
141 | Ki[0,0] += g_focal
142 | Ki[1,1] += g_focal
143 |
144 | img = ori_img.copy()
145 | # x取值范围0-10，间隔0.1
146 | for x in np.arange(0,10,0.5):
147 | for y in np.arange(-5,5,0.5):
148 | u,v = xy2uv(x,y,Ki,Ko)
149 | cv2.circle(img, (u,v), 3, (0,255,0), -1)
150 |
151 | # 修改img的大小
152 | img = cv2.resize(img, (int(width*0.5),int(height*0.5)))
153 | cv2.imshow('img', img)
154 | key = cv2.waitKey(50)
155 | if key in [ord('q'), 27] :
156 | break
157 |
158 | if args.test_only:
159 | return
160 |
161 | ki_save = []
162 | for item in Ki.tolist():
163 | ki_save.extend(item)
164 |
165 | ko_save = []
166 | for item in Ko.tolist():
167 | ko_save.extend(item)
168 |
169 | yaml.dump({
170 | "Ki": str(ki_save),
171 | "Ko": str(ko_save),
172 | "a1": cam_para.data.get("a1", 100.0),
173 | "a2": cam_para.data.get("a2", 100.0),
174 | "wx": cam_para.data.get("wx", 5.0),
175 | "wy": cam_para.data.get("wy", 5.0),
176 | "vmax": cam_para.data.get("vmax", 5.0),
177 | "max_age": cam_para.data.get("max_age", 10.0),
178 | "high_score": cam_para.data.get("high_score", 0.5),
179 | "conf_threshold": cam_para.data.get("conf_threshold", 0.01)
180 | }, open(args.cam_para, "w"), yaml.Dumper)
181 | save_str = open(args.cam_para).read()
182 | save_str = save_str.replace("'", "")
183 | open(args.cam_para, "w").write(save_str)
184 |
185 |
186 | if __name__ == "__main__":
187 | parser = argparse.ArgumentParser(description='Process some arguments.')
188 | parser.add_argument('-i', '--img', type=str, default=None, help='The image file')
189 | parser.add_argument('-v', '--vid', type=str, default=None, help='The video file')
190 | parser.add_argument('--id', type=int, default=0, help='The frame id of video')
191 | parser.add_argument('-f', '--cam_para', type=str, required=True,help='The estimated camera parameters file ')
192 | parser.add_argument("-t", "--test-only", action="store_true")
193 | args = parser.parse_args()
194 | assert (args.img or args.vid) is not None
195 | main(args)
196 |
--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include "argparse/argparser.h"
3 |
4 | #include "image_utils/tracker.h"
5 | #include "print_utils.h"
6 | #include "yaml-cpp/yaml.h"
7 |
8 | using namespace std;
9 |
10 |
11 | argsutil::argparser get_args(int argc, char* argv[]) {
12 | auto parser = argsutil::argparser("UCMCTrack argument parser.");
13 |
14 | parser.add_argument("result_file", "result file path")
15 | .add_option("-f", "--file", "track config file", "cfg/track.yaml")
16 | .add_option("-p", "--pause", "pause playing at first. default is false", false)
17 | // .add_option("-d", "--debug", "show some debug info", false)
18 | .add_help_option()
19 | .parse(argc, argv);
20 |
21 | return parser;
22 | }
23 |
24 | bool isnum(string s) {
25 | stringstream sin(s);
26 | double t;
27 | char p;
28 | if (!(sin >> t))
29 | return false;
30 | if (sin >> p)
31 | return false;
32 | else
33 | return true;
34 | }
35 |
36 | void decode_results(YAML::Node node, std::vector>& dist) {
37 | dist.clear();
38 | int total_frame = node["frame"].as();
39 | for (int i=0;i objs;
42 | std::vector> res = this_frame.as>>();
43 | for (auto re: res) {
44 | detect::Object obj;
45 | obj.rect.x = (int)re[0];
46 | obj.rect.y = (int)re[1];
47 | obj.rect.width = (int)re[2];
48 | obj.rect.height = (int)re[3];
49 | obj.prob = re[4];
50 | obj.label = (int)re[5];
51 | objs.push_back(obj);
52 | }
53 | dist.push_back(objs);
54 | }
55 | }
56 |
57 | int main(int argc, char* argv[]) {
58 | auto args = get_args(argc, argv);
59 |
60 | std::string result_file = args.get_argument_string("result_file");
61 | YAML::Node result_cfg = YAML::LoadFile(result_file);
62 | std::string source = result_cfg["source"].as();
63 | std::vector names = result_cfg["names"].as>();
64 | std::vector> results;
65 | decode_results(result_cfg, results);
66 |
67 | cv::VideoCapture cap;
68 | if (isnum(source))
69 | cap.open(atoi(source.c_str()));
70 | else
71 | cap.open(source);
72 |
73 | std::string track_config_file = args.get_option_string("--file");
74 | YAML::Node track_cfg = YAML::LoadFile(track_config_file);
75 |
76 | UCMC::Params track_params;
77 | track_params.a1 = track_cfg["a1"].as();
78 | track_params.a2 = track_cfg["a2"].as();
79 | track_params.wx = track_cfg["wx"].as();
80 | track_params.wy = track_cfg["wy"].as();
81 | track_params.vmax = track_cfg["vmax"].as();
82 | track_params.max_age = track_cfg["max_age"].as();
83 | track_params.high_score = track_cfg["high_score"].as();
84 | track_params.conf_threshold = track_cfg["conf_threshold"].as();
85 | track_params.dt = 1./ MAX(10, cap.get(cv::CAP_PROP_FPS));
86 | track_params.Ki = track_cfg["Ki"].as>();
87 | track_params.Ko = track_cfg["Ko"].as>();
88 |
89 | UCMC::Tracker tracker(track_params);
90 | // tracker.debug = args.get_option_bool("--debug");
91 | // tracker.mapper.debug = tracker.debug;
92 |
93 | cv::Mat frame;
94 | int key = -1;
95 | int delay = args.get_option_bool("--pause") ? 0 : 1;
96 | TimeCount t;
97 |
98 | float total_latency=0.;
99 | int frame_id=0;
100 | for(auto preds: results) {
101 | cap >> frame;
102 | if (frame.empty()) {
103 | // cout << "frame is empty, exit." << endl;
104 | cv::destroyAllWindows();
105 | break;
106 | }
107 |
108 | t.tic(0);
109 | std::vector track_result = tracker.update(preds);
110 | t.toc(0);
111 |
112 |
113 | frame = Track::draw_boxes(frame, track_result, names, 20, true);
114 |
115 | float latency=t.get_timeval_f(0);
116 | total_latency += latency;
117 | printf("%sframe %d ---> track latency: %.2f ms\n%s",
118 | GREEN,
119 | frame_id++,
120 | latency,
121 | END);
122 |
123 | cv::imshow("UCMCTrack result", frame);
124 | key = cv::waitKey(delay);
125 | switch (key) {
126 | case 27: // esc
127 | cap.release();
128 | break;
129 | case 32: // space
130 | delay = 1 - delay;
131 | break;
132 | default:
133 | break;
134 | }
135 | }
136 | cv::destroyAllWindows();
137 | printf("%saverage latency: %.2f ms%s\n",
138 | GREEN,
139 | total_latency / MAX(1, frame_id),
140 | END);
141 | return 0;
142 | }
143 |
--------------------------------------------------------------------------------