1 /*
2 * Copyright (c) 2017 Arm Limited.
3 *
4 * SPDX-License-Identifier: MIT
5 *
6 * Permission is hereby granted, free of charge, to any person obtaining a copy
7 * of this software and associated documentation files (the "Software"), to
8 * deal in the Software without restriction, including without limitation the
9 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
10 * sell copies of the Software, and to permit persons to whom the Software is
11 * furnished to do so, subject to the following conditions:
12 *
13 * The above copyright notice and this permission notice shall be included in all
14 * copies or substantial portions of the Software.
15 *
16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22 * SOFTWARE.
23 */
24 #include "arm_compute/core/CPP/kernels/CPPSortEuclideanDistanceKernel.h"
25
26 #include "arm_compute/core/Coordinates.h"
27 #include "arm_compute/core/Error.h"
28 #include "arm_compute/core/Helpers.h"
29 #include "arm_compute/core/TensorInfo.h"
30 #include "arm_compute/core/Types.h"
31 #include "arm_compute/core/Utils.h"
32 #include "arm_compute/core/Validate.h"
33 #include "arm_compute/core/Window.h"
34
35 #include <cmath>
36
37 using namespace arm_compute;
38
39 namespace
40 {
keypoint_compare(const InternalKeypoint & lhs,const InternalKeypoint & rhs)41 bool keypoint_compare(const InternalKeypoint &lhs, const InternalKeypoint &rhs)
42 {
43 return std::get<2>(lhs) > std::get<2>(rhs);
44 }
45 } // namespace
46
CPPSortEuclideanDistanceKernel()47 CPPSortEuclideanDistanceKernel::CPPSortEuclideanDistanceKernel()
48 : _num_corner_candidates(), _min_distance(0.0f), _in_out(nullptr), _output(nullptr)
49 {
50 }
51
configure(InternalKeypoint * in_out,IKeyPointArray * output,const int32_t * num_corner_candidates,float min_distance)52 void CPPSortEuclideanDistanceKernel::configure(InternalKeypoint *in_out, IKeyPointArray *output, const int32_t *num_corner_candidates, float min_distance)
53 {
54 ARM_COMPUTE_ERROR_ON(nullptr == in_out);
55 ARM_COMPUTE_ERROR_ON(nullptr == output);
56 ARM_COMPUTE_ERROR_ON(nullptr == num_corner_candidates);
57 ARM_COMPUTE_ERROR_ON(!((min_distance > 0) && (min_distance <= 30)));
58
59 _in_out = in_out;
60 _output = output;
61 _min_distance = min_distance * min_distance; // We compare squares of distances
62 _num_corner_candidates = num_corner_candidates;
63 ICPPKernel::configure(Window()); // Default 1 iteration window
64 }
65
is_parallelisable() const66 bool CPPSortEuclideanDistanceKernel::is_parallelisable() const
67 {
68 return false;
69 }
70
run(const Window & window,const ThreadInfo & info)71 void CPPSortEuclideanDistanceKernel::run(const Window &window, const ThreadInfo &info)
72 {
73 ARM_COMPUTE_UNUSED(info);
74 ARM_COMPUTE_UNUSED(window);
75 ARM_COMPUTE_ERROR_ON_UNCONFIGURED_KERNEL(this);
76 ARM_COMPUTE_ERROR_ON_MISMATCHING_WINDOWS(ICPPKernel::window(), window);
77
78 const int32_t num_corner_candidates = *_num_corner_candidates;
79
80 /* Sort list of corner candidates */
81 std::sort(_in_out, _in_out + num_corner_candidates, keypoint_compare);
82
83 /* Euclidean distance */
84 for(int32_t i = 0; i < num_corner_candidates; ++i)
85 {
86 if(std::get<2>(_in_out[i]) != 0.0f)
87 {
88 KeyPoint keypt;
89 const auto xc = std::get<0>(_in_out[i]);
90 const auto yc = std::get<1>(_in_out[i]);
91
92 keypt.x = xc;
93 keypt.y = yc;
94 keypt.strength = std::get<2>(_in_out[i]);
95 keypt.tracking_status = 1;
96
97 /* Store corner */
98 _output->push_back(keypt);
99 for(int32_t k = i + 1; k < num_corner_candidates; ++k)
100 {
101 const float dx = std::fabs(std::get<0>(_in_out[k]) - xc);
102 const float dy = std::fabs(std::get<1>(_in_out[k]) - yc);
103
104 if((dx < _min_distance) && (dy < _min_distance))
105 {
106 const float d = (dx * dx + dy * dy);
107
108 if(d < _min_distance)
109 {
110 /* Invalidate keypoint */
111 std::get<2>(_in_out[k]) = 0.0f;
112 }
113 }
114 }
115 }
116 }
117 }
118