Ocean
Loading...
Searching...
No Matches
PoseEstimationT.h
Go to the documentation of this file.
1/*
2 * Copyright (c) Meta Platforms, Inc. and affiliates.
3 *
4 * This source code is licensed under the MIT license found in the
5 * LICENSE file in the root directory of this source tree.
6 */
7
8#ifndef META_OCEAN_TRACKING_POSE_ESTIMATION_T_H
9#define META_OCEAN_TRACKING_POSE_ESTIMATION_T_H
10
11#include "ocean/tracking/Tracking.h"
12
13#include "ocean/base/Worker.h"
14
15#include "ocean/geometry/Octree.h"
16#include "ocean/geometry/RANSAC.h"
17
18#include "ocean/math/AnyCamera.h"
19#include "ocean/math/HomogenousMatrix4.h"
20#include "ocean/math/PinholeCamera.h"
21#include "ocean/math/Vector2.h"
22#include "ocean/math/Vector3.h"
23
24namespace Ocean
25{
26
27namespace Tracking
28{
29
30/**
31 * This class implements functions to estimate camera poses using template-based data types.
32 * @ingroup tracking
33 */
34class OCEAN_TRACKING_EXPORT PoseEstimationT
35{
36 public:
37
38 /**
39 * Definition of an unordered map mapping object point ids to descriptors.
40 */
41 template <typename TDescriptor>
42 using UnorderedDescriptorMap = std::unordered_map<Index32, TDescriptor>;
43
44 public:
45
46 /**
47 * Determines the 6-DOF pose for 2D/3D correspondences applying a brute-force search.
48 * The function can be used to verify the performance/correctness of pose estimation functions avoiding a brute-force search.
49 * @param camera The camera profile to be used, must be valid
50 * @param objectPoints The 3D object points of all possible 3D features, must be valid
51 * @param objectPointDescriptors The descriptors for all 3D object points, one descriptor for each 3D object point (however, TObjectPointDescriptor allows to e.g., define a array/vector of several descriptors per object point), must be valid
52 * @param numberObjectPoints The number of given 3D object points (and object point descriptors), with range [4, infinity)
53 * @param imagePoints The 2D image points of all possible 2D features, must be valid
54 * @param imagePointDescriptors The descriptors for all 2D image points, one for each image points, must be valid
55 * @param numberImagePoints The number of 2D image points, with range [4, infinity)
56 * @param randomGenerator The random generator to be used
57 * @param world_T_camera The resulting camera pose transforming camera to world, with default camera looking into the negative z-space an y-axis upwards
58 * @param minimalNumberCorrespondences The minimal number of 2D/3D correspondences so that a camera pose counts as valid, with range [4, infinity)
59 * @param maximalDescriptorDistance The maximal descriptor distance so that two descriptors are still considered to match, with range [0, infinity)
60 * @param maximalProjectionError The maximal projection error between a projected 3D object point and a 2D image point so that both points count as corresponding, in pixel, with range [0, infinity)
61 * @param inlierRate The rate of correspondence inliers within the entire set of correspondences, e.g., 0.15 means that 15% of matched features are correct and can be used to determine a valid pose, with range (0, 1]
62 * @param usedObjectPointIndices Optional resulting indices of the 3D object points which have been used to determine the camera pose, nullptr if not of interest
63 * @param usedImagePointIndices Optional resulting indices of the 2D image points which have been used to determine the camera pose, nullptr if not of interest
64 * @param world_T_roughCamera Optional known rough camera pose allowing to skip the unguided matching, invalid if unknown
65 * @param worker Optional worker to distribute the computation
66 * @return True, if succeeded
67 * @tparam TImagePointDescriptor The data type of the descriptor for the 2D image points
68 * @tparam TObjectPointDescriptor The data type of the descriptor for the 3D object points
69 * @tparam TDescriptorDistance The data type of the distance between image point and object point descriptors, e.g., 'uint32_t' or 'float'
70 * @tparam tImageObjectDistanceFunction The function pointer to a function allowing to determine the descriptor distance between an image point feature descriptor and an object point feature descriptor, must be valid
71 */
72 template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
73 static bool determinePoseBruteForce(const AnyCamera& camera, const Vector3* objectPoints, const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPoints, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, const size_t numberImagePoints, RandomGenerator& randomGenerator, HomogenousMatrix4& world_T_camera, const unsigned int minimalNumberCorrespondences, const TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, const Scalar inlierRate = Scalar(0.15), Indices32* usedObjectPointIndices = nullptr, Indices32* usedImagePointIndices = nullptr, const HomogenousMatrix4& world_T_roughCamera = HomogenousMatrix4(false), Worker* worker = nullptr);
74
75 /**
76 * Determines the 6-DOF pose for 2D/3D correspondences applying a brute-force search.
77 * The function can be used to verify the performance/correctness of pose estimation functions avoiding a brute-force search.
78 * This function uses descriptors for 3D object points which can be stored in an arbitrary order (even multiple individual descriptors can exist for one 3D object point).
79 * The relationship between object point descriptors and their corresponding 3D object point locations is defined via 'objectPointIndices'.
80 * @param camera The camera profile to be used, must be valid
81 * @param objectPoints The 3D object points of all possible 3D features, must be valid
82 * @param objectPointDescriptors The descriptors for all 3D object points, some descriptors may describe the same 3D object points (e.g., because of different viewing locations), must be valid
83 * @param objectPointIndices The indices of the corresponding 3D object points, one for each object point descriptor, mainly a map mapping descriptor indices to point indices, must be valid
84 * @param numberObjectDescriptors The number of given descriptors for the 3D object points, with range [4, infinity)
85 * @param imagePoints The 2D image points of all possible 2D features, must be valid
86 * @param imagePointDescriptors The descriptors for all 2D image points, one for each image points, must be valid
87 * @param numberImagePoints The number of 2D image points, with range [4, infinity)
88 * @param randomGenerator The random generator to be used
89 * @param world_T_camera The resulting camera pose transforming camera to world, with default camera looking into the negative z-space an y-axis upwards
90 * @param minimalNumberCorrespondences The minimal number of 2D/3D correspondences so that a camera pose counts as valid, with range [4, infinity)
91 * @param maximalDescriptorDistance The maximal descriptor distance so that two descriptors are still considered to match, with range [0, infinity)
92 * @param maximalProjectionError The maximal projection error between a projected 3D object point and a 2D image point so that both points count as corresponding, in pixel, with range [0, infinity)
93 * @param inlierRate The rate of correspondence inliers within the entire set of correspondences, e.g., 0.15 means that 15% of matched features are correct and can be used to determine a valid pose, with range (0, 1]
94 * @param usedObjectPointIndices Optional resulting indices of the 3D object points which have been used to determine the camera pose, nullptr if not of interest
95 * @param usedImagePointIndices Optional resulting indices of the 2D image points which have been used to determine the camera pose, nullptr if not of interest
96 * @param world_T_roughCamera Optional known rough camera pose allowing to skip the unguided matching, invalid if unknown
97 * @return True, if succeeded
98 * @tparam TImagePointDescriptor The data type of the descriptor for the 2D image points
99 * @tparam TObjectPointDescriptor The data type of the descriptor for the 3D object points
100 * @tparam TDescriptorDistance The data type of the distance between image point and object point descriptors, e.g., 'uint32_t' or 'float'
101 * @tparam tImageObjectDistanceFunction The function pointer to a function allowing to determine the descriptor distance between an image point feature descriptor and an object point feature descriptor, must be valid
102 */
103 template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
104 static bool determinePoseBruteForceWithArbitraryDescriptorOrder(const AnyCamera& camera, const Vector3* objectPoints, const TObjectPointDescriptor* objectPointDescriptors, const Index32* objectPointIndices, const size_t numberObjectDescriptors, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, const size_t numberImagePoints, RandomGenerator& randomGenerator, HomogenousMatrix4& world_T_camera, const unsigned int minimalNumberCorrespondences, const TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, const Scalar inlierRate = Scalar(0.15), Indices32* usedObjectPointIndices = nullptr, Indices32* usedImagePointIndices = nullptr, const HomogenousMatrix4& world_T_roughCamera = HomogenousMatrix4(false));
105
106 /**
107 * Determines the unguided matching (without apriori information) between image point and object point features applying a brute-force search.
108 * @param objectPointDescriptors The object point descriptors to be used, must be valid
109 * @param numberObjectPointDescriptors The number of given object point descriptors, with range [1, infinity)
110 * @param imagePointDescriptors The image point descriptors to be used, must be valid
111 * @param numberImagePointDescriptors The number of given image point descriptors, with range [1, infinity)
112 * @param maximalDescriptorDistance The maximal distance between two corresponding descriptors, with range [0, infinity)
113 * @param objectPointDescriptorIndices The resulting indices of object point descriptors matching to the individual image point descriptors, one object point descriptor index of each given image point descriptor, -1 for image point descriptors for which no matching object point descriptor could be found
114 * @param worker Optional worker to distribute the computation
115 * @param distances Optional resulting distances between the individual matched descriptors, one for each image point descriptor, undefined if no matching exists, nullptr if not of interest
116 * @tparam TImagePointDescriptor The data type of the descriptor for the 2D image points
117 * @tparam TObjectPointDescriptor The data type of the descriptor for the 3D object points
118 * @tparam TDescriptorDistance The data type of the distance between image point and object point descriptors, e.g., 'uint32_t' or 'float'
119 * @tparam tImageObjectDistanceFunction The function pointer to a function allowing to determine the descriptor distance between an image point feature descriptor and an object point feature descriptor, must be valid
120 */
121 template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
122 static void determineUnguidedBruteForceMatchings(const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPointDescriptors, const TImagePointDescriptor* imagePointDescriptors, const size_t numberImagePointDescriptors, TDescriptorDistance maximalDescriptorDistance, Index32* objectPointDescriptorIndices, Worker* worker, TDescriptorDistance* distances = nullptr);
123
124 /**
125 * Determines the guided matching (without known rough camera pose) between image point and object point features applying a brute-force search.
126 * @param camera The camera profile defining the projection, must be valid
127 * @param world_T_roughCamera The known rough camera pose transforming camera to world, must be valid
128 * @param objectPoints The 3D object points to be used, must be valid
129 * @param objectPointDescriptors The object point descriptors, one for each 3D object point, must be valid
130 * @param numberObjectPoints The number of given object points (and object point descriptors), with range [1, infinity)
131 * @param imagePoints The 2D image points to be used, must be valid
132 * @param imagePointDescriptors The image point descriptors, one for each 2D image point, must be valid
133 * @param numberImagePoints The number of given image points (and image point descriptors), with range [1, infinity)
134 * @param maximalDescriptorDistance The maximal distance between two corresponding descriptors, with range [0, infinity)
135 * @param maximalProjectionError The maximal projection error of valid 2D/3D feature correspondences, in pixel, with range [0, infinity)
136 * @param objectPointDescriptorIndices The resulting indices of object point descriptors matching to the individual image point descriptors, one object point descriptor index of each given image point descriptor, -1 for image point descriptors for which no matching object point descriptor could be found
137 * @param worker Optional worker to distribute the computation
138 * @tparam TImagePointDescriptor The data type of the descriptor for the 2D image points
139 * @tparam TObjectPointDescriptor The data type of the descriptor for the 3D object points
140 * @tparam TDescriptorDistance The data type of the distance between image point and object point descriptors, e.g., 'uint32_t' or 'float'
141 * @tparam tImageObjectDistanceFunction The function pointer to a function allowing to determine the descriptor distance between an image point feature descriptor and an object point feature descriptor, must be valid
142 */
143 template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
144 static void determineGuidedBruteForceMatchings(const AnyCamera& camera, const HomogenousMatrix4& world_T_roughCamera, const Vector3* objectPoints, const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPoints, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, const size_t numberImagePoints, TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, Index32* objectPointDescriptorIndices, Worker* worker);
145
146 /**
147 * Determines the guided matching between 2D/3D correspondences.
148 * @param camera The camera profile to be used, must be valid
149 * @param world_T_camera The known (rough) camera pose transforming camera to world, with default camera looking into the negative z-space an y-axis upwards, must be valid
150 * @param imagePoints The 2D image points of all possible 2D features, must be valid
151 * @param imagePointDescriptors The descriptors for all 2D image points, one for each image points, must be valid
152 * @param numberImagePoints The number of 2D image points, with range [4, infinity)
153 * @param objectPoints The 3D object points of all possible 3D features, must be valid
154 * @param objectPointOctree The octree with all 3D object points
155 * @param objectPointIds The ids of all 3D object points, one for each 3D object point, must be valid
156 * @param objectPointDescriptorMap The map mapping 3D object point ids to their descriptors
157 * @param matchedImagePoints The resulting 2D image points which have been determined during the matching
158 * @param matchedObjectPoints The resulting 3D object points which have been determined during the matching
159 * @param maximalDescriptorDistance The maximal descriptor distance so that two descriptors are still considered to match, with range [0, infinity)
160 * @param matchedImagePointIndices Optional resulting indices of the image points which have been matched, one for each resulting 2D image points, nullptr if not of interest
161 * @param matchedObjectPointIds Optional resulting object point ids of the object points which have been matched, one for each resulting 3D image points, nullptr if not of interest
162 * @param worker Optional worker to distribute the computation
163 * @tparam TImagePointDescriptor The data type of the descriptor for the 2D image points
164 * @tparam TObjectPointDescriptor The data type of the descriptor for the 3D object points
165 * @tparam TDescriptorDistance The data type of the distance between image point and object point descriptors, e.g., 'uint32_t' or 'float'
166 * @tparam tImageObjectDistanceFunction The function pointer to a function allowing to determine the descriptor distance between an image point feature descriptor and an object point feature descriptor, must be valid
167 */
168 template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
169 static void determineGuidedMatchings(const AnyCamera& camera, const HomogenousMatrix4& world_T_camera, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, const size_t numberImagePoints, const Vector3* objectPoints, const Geometry::Octree& objectPointOctree, const Index32* objectPointIds, const UnorderedDescriptorMap<TObjectPointDescriptor>& objectPointDescriptorMap, Vectors2& matchedImagePoints, Vectors3& matchedObjectPoints, const TDescriptorDistance maximalDescriptorDistance, Indices32* matchedImagePointIndices = nullptr, Indices32* matchedObjectPointIds = nullptr, Worker* worker = nullptr);
170
171 /**
172 * Returns the individual descriptor distances for a given set of corresponding feature descriptor pairs.
173 * This function can be used to e.g., analyze the distribution of descriptor distances and e.g., to determine an ideal threshold for valid descriptor correspondences.
174 * @param objectPointDescriptors The descriptors of the object points, must be valid
175 * @param imagePointDescriptors The descriptors of the image points, must be valid
176 * @param objectPointDescriptorIndices The indices of the object point descriptors for which a corresponding image point descriptor exists, must be valid
177 * @param imagePointDescriptorIndices The indices of the image point descriptors for which a corresponding object point descriptor exists, one for each object point descriptor, with the same order as the object point descriptors, must be valid
178 * @param numberCorrespondences The number of specified descriptor correspondences, with range [1, infinity)
179 * @return The descriptor distances, one of each corresponding descriptor pair, in the same order as the provided correspondences
180 * @tparam TImagePointDescriptor The data type of the descriptor for the 2D image points
181 * @tparam TObjectPointDescriptor The data type of the descriptor for the 3D object points
182 * @tparam TDescriptorDistance The data type of the distance between image point and object point descriptors, e.g., 'uint32_t' or 'float'
183 * @tparam tImageObjectDistanceFunction The function pointer to a function allowing to determine the descriptor distance between an image point feature descriptor and an object point feature descriptor, must be valid
184 */
185 template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
186 static std::vector<TDescriptorDistance> determineDescriptorDistances(const TObjectPointDescriptor* objectPointDescriptors, const TImagePointDescriptor* imagePointDescriptors, const Index32* objectPointDescriptorIndices, const Index32* imagePointDescriptorIndices, const size_t numberCorrespondences);
187
188 protected:
189
190 /**
191 * Determines the unguided matching between a subset of image point and object point features applying a brute-force search.
192 * @param objectPointDescriptors The object point descriptors to be used, must be valid
193 * @param numberObjectPointDescriptors The number of given object point descriptors, with range [1, infinity)
194 * @param imagePointDescriptors The image point descriptors to be used, must be valid
195 * @param maximalDescriptorDistance The maximal distance between two corresponding descriptors, with range [0, infinity)
196 * @param objectPointDescriptorIndices The resulting indices of object point descriptors matching to the individual image point descriptors, one object point descriptor index of each given image point descriptor, -1 for image point descriptors for which no matching object point descriptor could be found
197 * @param distances Optional resulting distances between the individual matched descriptors, one for each image point descriptor, nullptr if not of interest
198 * @param firstImagePointDescriptor The first image point descriptor to be handled, with range [0, numberImagePointDescriptors - 1]
199 * @param numberImagePointDescriptors The number of image point descriptors to be handled, with range [1, numberImagePointDescriptors - firstImagePointDescriptor]
200 * @tparam TImagePointDescriptor The data type of the descriptor for the 2D image points
201 * @tparam TObjectPointDescriptor The data type of the descriptor for the 3D object points
202 * @tparam TDescriptorDistance The data type of the distance between image point and object point descriptors, e.g., 'uint32_t' or 'float'
203 * @tparam tImageObjectDistanceFunction The function pointer to a function allowing to determine the descriptor distance between an image point feature descriptor and an object point feature descriptor, must be valid
204 */
205 template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
206 static void determineUnguidedBruteForceMatchingsSubset(const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPointDescriptors, const TImagePointDescriptor* imagePointDescriptors, TDescriptorDistance maximalDescriptorDistance, Index32* objectPointDescriptorIndices, TDescriptorDistance* distances, const unsigned int firstImagePointDescriptor, const unsigned int numberImagePointDescriptors);
207
208 /**
209 * Determines the guided matching (without known rough camera pose) between a subset of image point and object point features applying a brute-force search.
210 * @param camera The camera profile defining the projection, must be valid
211 * @param world_T_roughCamera The known rough camera pose transforming camera to world, must be valid
212 * @param objectPoints The 3D object points to be used, must be valid
213 * @param objectPointDescriptors The object point descriptors, one for each 3D object point, must be valid
214 * @param numberObjectPoints The number of given object points (and object point descriptors), with range [1, infinity)
215 * @param imagePoints The 2D image points to be used, must be valid
216 * @param imagePointDescriptors The image point descriptors, one for each 2D image point, must be valid
217 * @param maximalDescriptorDistance The maximal distance between two corresponding descriptors, with range [0, infinity)
218 * @param maximalProjectionError The maximal projection error of valid 2D/3D feature correspondences, in pixel, with range [0, infinity)
219 * @param objectPointDescriptorIndices The resulting indices of object point descriptors matching to the individual image point descriptors, one object point descriptor index of each given image point descriptor, -1 for image point descriptors for which no matching object point descriptor could be found
220 * @param firstImagePoint The first image point to be handled, with range [0, numberImagePoints - 1]
221 * @param numberImagePoints The number of image points to be handled, with range [1, numberImagePoints - firstImagePoint]
222 * @tparam TImagePointDescriptor The data type of the descriptor for the 2D image points
223 * @tparam TObjectPointDescriptor The data type of the descriptor for the 3D object points
224 * @tparam TDescriptorDistance The data type of the distance between image point and object point descriptors, e.g., 'uint32_t' or 'float'
225 * @tparam tImageObjectDistanceFunction The function pointer to a function allowing to determine the descriptor distance between an image point feature descriptor and an object point feature descriptor, must be valid
226 */
227 template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
228 static void determineGuidedBruteForceMatchingsSubset(const AnyCamera* camera, const HomogenousMatrix4* world_T_roughCamera, const Vector3* objectPoints, const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPoints, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, Index32* objectPointDescriptorIndices, const unsigned int firstImagePoint, const unsigned int numberImagePoints);
229
230 /**
231 * Determines the guided matching between 2D/3D correspondences for a subset of the given 2D image points.
232 * @param camera The camera profile to be used, must be valid
233 * @param world_T_camera The known (rough) camera pose transforming camera to world, with default camera looking into the negative z-space an y-axis upwards, must be valid
234 * @param imagePoints The 2D image points of all possible 2D features, must be valid
235 * @param imagePointDescriptors The descriptors for all 2D image points, one for each image points, must be valid
236 * @param objectPoints The 3D object points of all possible 3D features, must be valid
237 * @param objectPointOctree The octree with all 3D object points
238 * @param objectPointIds The ids of all 3D object points, one for each 3D object point, must be valid
239 * @param objectPointDescriptorMap The map mapping 3D object point ids to their descriptors
240 * @param matchedImagePoints The resulting 2D image points which have been determined during the matching
241 * @param matchedObjectPoints The resulting 3D object points which have been determined during the matching
242 * @param maximalDescriptorDistance The maximal descriptor distance so that two descriptors are still considered to match, with range [0, infinity)
243 * @param matchedImagePointIndices The resulting indices of the image points which have been matched, one for each resulting 2D image points, nullptr if not of interest
244 * @param matchedObjectPointIds The resulting object point ids of the object points which have been matched, one for each resulting 3D image points, nullptr if not of interest
245 * @param lock The lock object in case this function is executed across multiple thread, nullptr if executed in one thread
246 * @param firstImagePoint The index of the first image point to be handled, with range [0, numberImagePoints - 1]
247 * @param numberImagePoints The number of image points to be handled, with range [1, numberImagePoints - firstImagePoint]
248 * @tparam TImagePointDescriptor The data type of the descriptor for the 2D image points
249 * @tparam TObjectPointDescriptor The data type of the descriptor for the 3D object points
250 * @tparam TDescriptorDistance The data type of the distance between image point and object point descriptors, e.g., 'uint32_t' or 'float'
251 * @tparam tImageObjectDistanceFunction The function pointer to a function allowing to determine the descriptor distance between an image point feature descriptor and an object point feature descriptor, must be valid
252 */
253 template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
254 static void determineGuidedMatchingsSubset(const AnyCamera* camera, const HomogenousMatrix4* world_T_camera, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, const Vector3* objectPoints, const Geometry::Octree* objectPointOctree, const Index32* objectPointIds, const UnorderedDescriptorMap<TObjectPointDescriptor>* objectPointDescriptorMap, Vectors2* matchedImagePoints, Vectors3* matchedObjectPoints, Indices32* matchedImagePointIndices, Indices32* matchedObjectPointIds, const TDescriptorDistance maximalDescriptorDistance, Lock* lock, const unsigned int firstImagePoint, const unsigned int numberImagePoints);
255};
256
257template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
258bool PoseEstimationT::determinePoseBruteForce(const AnyCamera& camera, const Vector3* objectPoints, const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPoints, const Vector2* imagePoints, const TImagePointDescriptor
259* imagePointDescriptors, const size_t numberImagePoints, RandomGenerator& randomGenerator, HomogenousMatrix4& world_T_camera, const unsigned int minimalNumberCorrespondences, const TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, const Scalar inlierRate, Indices32* usedObjectPointIndices, Indices32* usedImagePointIndices, const HomogenousMatrix4& world_T_roughCamera, Worker* worker)
260{
261 ocean_assert(camera.isValid());
262
263 ocean_assert(maximalProjectionError >= Scalar(0));
264 ocean_assert(inlierRate > Scalar(0) && inlierRate <= Scalar(1));
265
266 if (numberImagePoints < size_t(minimalNumberCorrespondences))
267 {
268 return false;
269 }
270
271 const Scalar maximalSqrProjectionError = maximalProjectionError * maximalProjectionError;
272
273 Indices32 matchedObjectPointIds(numberImagePoints);
274
275 Vectors2 matchedImagePoints;
276 Vectors3 matchedObjectPoints;
277
278 matchedImagePoints.reserve(numberImagePoints);
279 matchedObjectPoints.reserve(numberImagePoints);
280
281 if (!world_T_roughCamera.isValid())
282 {
283 determineUnguidedBruteForceMatchings<TImagePointDescriptor, TObjectPointDescriptor, TDescriptorDistance, tImageObjectDistanceFunction>(objectPointDescriptors, numberObjectPoints, imagePointDescriptors, numberImagePoints, maximalDescriptorDistance, matchedObjectPointIds.data(), worker);
284
285 for (size_t imagePointIndex = 0; imagePointIndex < numberImagePoints; ++imagePointIndex)
286 {
287 const Index32& matchedObjectPointIndex = matchedObjectPointIds[imagePointIndex];
288
289 if (matchedObjectPointIndex != Index32(-1))
290 {
291 matchedImagePoints.emplace_back(imagePoints[imagePointIndex]);
292 matchedObjectPoints.emplace_back(objectPoints[matchedObjectPointIndex]);
293 }
294 }
295
296 if (matchedImagePoints.size() < size_t(minimalNumberCorrespondences))
297 {
298 world_T_camera.toNull();
299 return false;
300 }
301 }
302
303 Indices32 internalUsedObjectPointIndices;
304 Indices32 internalUsedImagePointIndices;
305
306 const bool useInternalIndices = usedObjectPointIndices != nullptr || usedImagePointIndices != nullptr;
307
308 if (useInternalIndices)
309 {
310 internalUsedObjectPointIndices.reserve(numberImagePoints);
311 internalUsedImagePointIndices.reserve(numberImagePoints);
312 }
313
314 const Scalar faultyRate = Scalar(1) - inlierRate;
315 ocean_assert(faultyRate >= Scalar(0) && faultyRate < Scalar(1));
316
317 const unsigned int unguidedIterations = Geometry::RANSAC::iterations(3u, Scalar(0.99), faultyRate);
318
319 Indices32 validIndices;
320 if (world_T_roughCamera.isValid() || Geometry::RANSAC::p3p(camera, ConstArrayAccessor<Vector3>(matchedObjectPoints), ConstArrayAccessor<Vector2>(matchedImagePoints), randomGenerator, world_T_camera, 20u, true, unguidedIterations, maximalSqrProjectionError, &validIndices))
321 {
322 // now applying guided matching
323
324 if (world_T_roughCamera.isValid())
325 {
326 world_T_camera = world_T_roughCamera;
327 }
328
329 matchedImagePoints.clear();
330 matchedObjectPoints.clear();
331
332 determineGuidedBruteForceMatchings<TImagePointDescriptor, TObjectPointDescriptor, TDescriptorDistance, tImageObjectDistanceFunction>(camera, world_T_camera, objectPoints, objectPointDescriptors, numberObjectPoints, imagePoints, imagePointDescriptors, numberImagePoints, maximalDescriptorDistance, maximalProjectionError, matchedObjectPointIds.data(), worker);
333
334 for (size_t imagePointIndex = 0; imagePointIndex < numberImagePoints; ++imagePointIndex)
335 {
336 const Index32& matchedObjectPointIndex = matchedObjectPointIds[imagePointIndex];
337
338 if (matchedObjectPointIndex != Index32(-1))
339 {
340 matchedImagePoints.emplace_back(imagePoints[imagePointIndex]);
341 matchedObjectPoints.emplace_back(objectPoints[matchedObjectPointIndex]);
342
343 if (useInternalIndices)
344 {
345 internalUsedImagePointIndices.emplace_back(Index32(imagePointIndex));
346 internalUsedObjectPointIndices.emplace_back(matchedObjectPointIndex);
347 }
348 }
349 }
350
351 if (matchedImagePoints.size() < size_t(minimalNumberCorrespondences))
352 {
353 world_T_camera.toNull();
354 return false;
355 }
356
357 constexpr unsigned int guidedIterations = 40u;
358
359 world_T_camera.toNull();
360 validIndices.clear();
361 if (Geometry::RANSAC::p3p(camera, ConstArrayAccessor<Vector3>(matchedObjectPoints), ConstArrayAccessor<Vector2>(matchedImagePoints), randomGenerator, world_T_camera, 20u, true, guidedIterations, maximalSqrProjectionError, &validIndices))
362 {
363 if (usedObjectPointIndices != nullptr)
364 {
365 ocean_assert(useInternalIndices);
366
367 usedObjectPointIndices->clear();
368 usedObjectPointIndices->reserve(validIndices.size());
369
370 for (const Index32 validIndex : validIndices)
371 {
372 usedObjectPointIndices->emplace_back(internalUsedObjectPointIndices[validIndex]);
373 }
374 }
375
376 if (usedImagePointIndices != nullptr)
377 {
378 ocean_assert(useInternalIndices);
379
380 usedImagePointIndices->clear();
381 usedImagePointIndices->reserve(validIndices.size());
382
383 for (const Index32 validIndex : validIndices)
384 {
385 usedImagePointIndices->emplace_back(internalUsedImagePointIndices[validIndex]);
386 }
387 }
388
389 return true;
390 }
391 }
392
393 world_T_camera.toNull();
394
395 return false;
396}
397
398template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
399bool PoseEstimationT::determinePoseBruteForceWithArbitraryDescriptorOrder(const AnyCamera& camera, const Vector3* objectPoints, const TObjectPointDescriptor* objectPointDescriptors, const Index32* objectPointIndices, const size_t numberObjectDescriptors, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, const size_t numberImagePoints, RandomGenerator& randomGenerator, HomogenousMatrix4& world_T_camera, const unsigned int minimalNumberCorrespondences, const TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, const Scalar inlierRate, Indices32* usedObjectPointIndices, Indices32* usedImagePointIndices, const HomogenousMatrix4& world_T_roughCamera)
400{
401 ocean_assert(camera.isValid());
402
403 ocean_assert(maximalProjectionError >= Scalar(0));
404 ocean_assert(inlierRate > Scalar(0) && inlierRate <= Scalar(1));
405
406 if (numberImagePoints < size_t(minimalNumberCorrespondences))
407 {
408 return false;
409 }
410
411 const Scalar maximalSqrProjectionError = maximalProjectionError * maximalProjectionError;
412
413 Vectors2 matchedImagePoints;
414 Vectors3 matchedObjectPoints;
415
416 matchedImagePoints.reserve(numberImagePoints);
417 matchedObjectPoints.reserve(numberImagePoints);
418
419 if (!world_T_roughCamera.isValid())
420 {
421 // unguided brute force matching
422
423 for (size_t imagePointIndex = 0; imagePointIndex < numberImagePoints; ++imagePointIndex)
424 {
425 const TImagePointDescriptor& imagePointDescriptor = imagePointDescriptors[imagePointIndex];
426
427 TDescriptorDistance bestDistance = NumericT<TDescriptorDistance>::maxValue();
428 Index32 bestObjectPointDescriptorIndex = Index32(-1);
429
430 for (size_t nObjectPoint = 0; nObjectPoint < numberObjectDescriptors; ++nObjectPoint)
431 {
432 const TDescriptorDistance distance = tImageObjectDistanceFunction(imagePointDescriptor, objectPointDescriptors[nObjectPoint]);
433
434 if (distance < bestDistance)
435 {
436 bestDistance = distance;
437 bestObjectPointDescriptorIndex = Index32(nObjectPoint);
438 }
439 }
440
441 if (bestDistance <= maximalDescriptorDistance)
442 {
443 const Index32 objectPointIndex = objectPointIndices[bestObjectPointDescriptorIndex];
444
445 matchedImagePoints.emplace_back(imagePoints[imagePointIndex]);
446 matchedObjectPoints.emplace_back(objectPoints[objectPointIndex]);
447 }
448 }
449
450 if (matchedImagePoints.size() < size_t(minimalNumberCorrespondences))
451 {
452 world_T_camera.toNull();
453 return false;
454 }
455 }
456
457 Indices32 internalUsedObjectPointIndices;
458 Indices32 internalUsedImagePointIndices;
459
460 const bool useInternalIndices = usedObjectPointIndices != nullptr || usedImagePointIndices != nullptr;
461
462 if (useInternalIndices)
463 {
464 internalUsedObjectPointIndices.reserve(numberImagePoints);
465 internalUsedImagePointIndices.reserve(numberImagePoints);
466 }
467
468 const Scalar faultyRate = Scalar(1) - inlierRate;
469 ocean_assert(faultyRate >= Scalar(0) && faultyRate < Scalar(1));
470
471 const unsigned int unguidedIterations = Geometry::RANSAC::iterations(3u, Scalar(0.99), faultyRate);
472
473 Indices32 validIndices;
474 if (world_T_roughCamera.isValid() || Geometry::RANSAC::p3p(camera, ConstArrayAccessor<Vector3>(matchedObjectPoints), ConstArrayAccessor<Vector2>(matchedImagePoints), randomGenerator, world_T_camera, 20u, true, unguidedIterations, maximalSqrProjectionError, &validIndices))
475 {
476 // now applying guided matching
477
478 if (world_T_roughCamera.isValid())
479 {
480 world_T_camera = world_T_roughCamera;
481 }
482
483 matchedImagePoints.clear();
484 matchedObjectPoints.clear();
485
486 const HomogenousMatrix4 flippedCamera_T_world(PinholeCamera::standard2InvertedFlipped(world_T_camera));
487
488 for (size_t imagePointIndex = 0; imagePointIndex < numberImagePoints; ++imagePointIndex)
489 {
490 const Vector2& imagePoint = imagePoints[imagePointIndex];
491 const TImagePointDescriptor& imagePointDescriptor = imagePointDescriptors[imagePointIndex];
492
493 TDescriptorDistance bestDistance = NumericT<TDescriptorDistance>::maxValue();
494 Index32 bestObjectPointDescriptorIndex = Index32(-1);
495
496 for (size_t nObjectPoint = 0; nObjectPoint < numberObjectDescriptors; ++nObjectPoint)
497 {
498 const TDescriptorDistance distance = tImageObjectDistanceFunction(imagePointDescriptor, objectPointDescriptors[nObjectPoint]);
499
500 if (distance < bestDistance)
501 {
502 const Index32 objectPointIndex = objectPointIndices[nObjectPoint];
503
504 const Vector3& objectPoint = objectPoints[objectPointIndex];
505
506 if (PinholeCamera::isObjectPointInFrontIF(flippedCamera_T_world, objectPoint) && camera.projectToImageIF(flippedCamera_T_world, objectPoint).sqrDistance(imagePoint) <= Scalar(20 * 20))
507 {
508 bestDistance = distance;
509 bestObjectPointDescriptorIndex = Index32(nObjectPoint);
510 }
511 }
512 }
513
514 if (bestDistance <= maximalDescriptorDistance)
515 {
516 const Index32 objectPointIndex = objectPointIndices[bestObjectPointDescriptorIndex];
517
518 matchedImagePoints.emplace_back(imagePoints[imagePointIndex]);
519 matchedObjectPoints.emplace_back(objectPoints[objectPointIndex]);
520
521 if (useInternalIndices)
522 {
523 internalUsedObjectPointIndices.emplace_back(objectPointIndex);
524 internalUsedImagePointIndices.emplace_back(Index32(imagePointIndex));
525 }
526 }
527 }
528
529 if (matchedImagePoints.size() < size_t(minimalNumberCorrespondences))
530 {
531 world_T_camera.toNull();
532 return false;
533 }
534
535 constexpr unsigned int guidedIterations = 40u;
536
537 world_T_camera.toNull();
538 validIndices.clear();
539 if (Geometry::RANSAC::p3p(camera, ConstArrayAccessor<Vector3>(matchedObjectPoints), ConstArrayAccessor<Vector2>(matchedImagePoints), randomGenerator, world_T_camera, 20u, true, guidedIterations, maximalSqrProjectionError, &validIndices))
540 {
541 if (usedObjectPointIndices != nullptr)
542 {
543 ocean_assert(useInternalIndices);
544
545 usedObjectPointIndices->clear();
546 usedObjectPointIndices->reserve(validIndices.size());
547
548 for (const Index32 validIndex : validIndices)
549 {
550 usedObjectPointIndices->emplace_back(internalUsedObjectPointIndices[validIndex]);
551 }
552 }
553
554 if (usedImagePointIndices != nullptr)
555 {
556 ocean_assert(useInternalIndices);
557
558 usedImagePointIndices->clear();
559 usedImagePointIndices->reserve(validIndices.size());
560
561 for (const Index32 validIndex : validIndices)
562 {
563 usedImagePointIndices->emplace_back(internalUsedImagePointIndices[validIndex]);
564 }
565 }
566
567 return true;
568 }
569 }
570
571 world_T_camera.toNull();
572
573 return false;
574}
575
576template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
577void PoseEstimationT::determineUnguidedBruteForceMatchings(const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPointDescriptors, const TImagePointDescriptor* imagePointDescriptors, const size_t numberImagePointDescriptors, const TDescriptorDistance maximalDescriptorDistance, Index32* objectPointDescriptorIndices, Worker* worker, TDescriptorDistance* distances)
578{
579 ocean_assert(objectPointDescriptors != nullptr);
580 ocean_assert(imagePointDescriptors != nullptr);
581 ocean_assert(objectPointDescriptorIndices != nullptr);
582
583 if (worker && (numberObjectPointDescriptors >= 1000 || numberImagePointDescriptors >= 1000))
584 {
585 worker->executeFunction(Worker::Function::createStatic(&determineUnguidedBruteForceMatchingsSubset<TImagePointDescriptor, TObjectPointDescriptor, TDescriptorDistance, tImageObjectDistanceFunction>, objectPointDescriptors, numberObjectPointDescriptors, imagePointDescriptors, maximalDescriptorDistance, objectPointDescriptorIndices, distances, 0u, 0u), 0u, (unsigned int)(numberImagePointDescriptors));
586 }
587 else
588 {
589 determineUnguidedBruteForceMatchingsSubset<TImagePointDescriptor, TObjectPointDescriptor, TDescriptorDistance, tImageObjectDistanceFunction>(objectPointDescriptors, numberObjectPointDescriptors, imagePointDescriptors, maximalDescriptorDistance, objectPointDescriptorIndices, distances, 0u, (unsigned int)(numberImagePointDescriptors));
590 }
591}
592
593template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
594void PoseEstimationT::determineGuidedBruteForceMatchings(const AnyCamera& camera, const HomogenousMatrix4& world_T_roughCamera, const Vector3* objectPoints, const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPoints, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, const size_t numberImagePoints, TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, Index32* objectPointDescriptorIndices, Worker* worker)
595{
596 ocean_assert(camera.isValid());
597 ocean_assert(world_T_roughCamera.isValid());
598 ocean_assert(objectPoints != nullptr);
599 ocean_assert(objectPointDescriptors != nullptr);
600 ocean_assert(imagePoints != nullptr);
601 ocean_assert(imagePointDescriptors != nullptr);
602 ocean_assert(objectPointDescriptorIndices != nullptr);
603 ocean_assert(maximalProjectionError >= 0);
604
605 if (worker && (numberObjectPoints >= 1000 || numberImagePoints >= 1000))
606 {
607 worker->executeFunction(Worker::Function::createStatic(&determineGuidedBruteForceMatchingsSubset<TImagePointDescriptor, TObjectPointDescriptor, TDescriptorDistance, tImageObjectDistanceFunction>, &camera, &world_T_roughCamera, objectPoints, objectPointDescriptors, numberObjectPoints, imagePoints, imagePointDescriptors, maximalDescriptorDistance, maximalProjectionError, objectPointDescriptorIndices, 0u, 0u), 0u, (unsigned int)(numberImagePoints));
608 }
609 else
610 {
611 determineGuidedBruteForceMatchingsSubset<TImagePointDescriptor, TObjectPointDescriptor, TDescriptorDistance, tImageObjectDistanceFunction>(&camera, &world_T_roughCamera, objectPoints, objectPointDescriptors, numberObjectPoints, imagePoints, imagePointDescriptors, maximalDescriptorDistance, maximalProjectionError, objectPointDescriptorIndices, 0u, (unsigned int)(numberImagePoints));
612 }
613}
614
615template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
616void PoseEstimationT::determineUnguidedBruteForceMatchingsSubset(const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPointDescriptors, const TImagePointDescriptor* imagePointDescriptors, const TDescriptorDistance maximalDescriptorDistance, Index32* objectPointDescriptorIndices, TDescriptorDistance* distances, const unsigned int firstImagePointDescriptor, const unsigned int numberImagePointDescriptors)
617{
618 ocean_assert(objectPointDescriptors != nullptr);
619 ocean_assert(imagePointDescriptors != nullptr);
620
621 for (size_t imagePointIndex = size_t(firstImagePointDescriptor); imagePointIndex < size_t(firstImagePointDescriptor + numberImagePointDescriptors); ++imagePointIndex)
622 {
623 const TImagePointDescriptor& imagePointDescriptor = imagePointDescriptors[imagePointIndex];
624
625 TDescriptorDistance bestDistance = NumericT<TDescriptorDistance>::maxValue();
626 Index32 bestObjectPointIndex = Index32(-1);
627
628 for (size_t objectPointIndex = 0; objectPointIndex < numberObjectPointDescriptors; ++objectPointIndex)
629 {
630 const TDescriptorDistance distance = tImageObjectDistanceFunction(imagePointDescriptor, objectPointDescriptors[objectPointIndex]);
631
632 if (distance < bestDistance)
633 {
634 bestDistance = distance;
635 bestObjectPointIndex = Index32(objectPointIndex);
636 }
637 }
638
639 if (bestDistance <= maximalDescriptorDistance)
640 {
641 objectPointDescriptorIndices[imagePointIndex] = bestObjectPointIndex;
642
643 if (distances != nullptr)
644 {
645 distances[imagePointIndex] = bestDistance;
646 }
647 }
648 else
649 {
650 objectPointDescriptorIndices[imagePointIndex] = Index32(-1);
651 }
652 }
653}
654
655template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
656void PoseEstimationT::determineGuidedBruteForceMatchingsSubset(const AnyCamera* camera, const HomogenousMatrix4* world_T_roughCamera, const Vector3* objectPoints, const TObjectPointDescriptor* objectPointDescriptors, const size_t numberObjectPoints, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, Index32* objectPointDescriptorIndices, const unsigned int firstImagePoint, const unsigned int numberImagePoints)
657{
658 ocean_assert(camera != nullptr && camera->isValid());
659 ocean_assert(world_T_roughCamera != nullptr && world_T_roughCamera->isValid());
660 ocean_assert(objectPoints != nullptr);
661 ocean_assert(objectPointDescriptors != nullptr);
662 ocean_assert(imagePoints != nullptr);
663 ocean_assert(imagePointDescriptors != nullptr);
664 ocean_assert(maximalProjectionError >= 0);
665
666 const Scalar maximalSqrProjectionError = Numeric::sqr(maximalProjectionError);
667
668 const HomogenousMatrix4 flippedCamera_T_world(AnyCamera::standard2InvertedFlipped(*world_T_roughCamera));
669
670 for (size_t imagePointIndex = size_t(firstImagePoint); imagePointIndex < size_t(firstImagePoint + numberImagePoints); ++imagePointIndex)
671 {
672 const Vector2& imagePoint = imagePoints[imagePointIndex];
673 ocean_assert(camera->isInside(imagePoint));
674
675 const TImagePointDescriptor& imagePointDescriptor = imagePointDescriptors[imagePointIndex];
676
677 TDescriptorDistance bestDistance = NumericT<TDescriptorDistance>::maxValue();
678 Index32 bestObjectPointIndex = Index32(-1);
679
680 for (size_t objectPointIndex = 0; objectPointIndex < numberObjectPoints; ++objectPointIndex)
681 {
682 const TDescriptorDistance distance = tImageObjectDistanceFunction(imagePointDescriptor, objectPointDescriptors[objectPointIndex]);
683
684 if (distance < bestDistance)
685 {
686 const Vector3& objectPoint = objectPoints[objectPointIndex];
687
688 if (PinholeCamera::isObjectPointInFrontIF(flippedCamera_T_world, objectPoint) && camera->projectToImageIF(flippedCamera_T_world, objectPoint).sqrDistance(imagePoint) <= maximalSqrProjectionError)
689 {
690 bestDistance = distance;
691 bestObjectPointIndex = Index32(objectPointIndex);
692 }
693 }
694 }
695
696 if (bestDistance <= maximalDescriptorDistance)
697 {
698 objectPointDescriptorIndices[imagePointIndex] = bestObjectPointIndex;
699 }
700 else
701 {
702 objectPointDescriptorIndices[imagePointIndex] = Index32(-1);
703 }
704 }
705}
706
707template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
708void PoseEstimationT::determineGuidedMatchings(const AnyCamera& camera, const HomogenousMatrix4& world_T_camera, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, const size_t numberImagePoints, const Vector3* objectPoints, const Geometry::Octree& objectPointOctree, const Index32* objectPointIds, const UnorderedDescriptorMap<TObjectPointDescriptor>& objectPointDescriptorMap, Vectors2& matchedImagePoints, Vectors3& matchedObjectPoints, const TDescriptorDistance maximalDescriptorDistance, Indices32* matchedImagePointIndices, Indices32* matchedObjectPointIds, Worker* worker)
709{
710 ocean_assert(camera.isValid() && world_T_camera.isValid());
711 ocean_assert(imagePoints != nullptr && imagePointDescriptors != nullptr && numberImagePoints >= 1);
712 ocean_assert(objectPoints != nullptr && objectPointIds != nullptr);
713
714 ocean_assert(matchedImagePoints.empty());
715 ocean_assert(matchedObjectPoints.empty());
716
717 if (matchedObjectPointIds != nullptr)
718 {
719 matchedObjectPointIds->clear();
720 }
721
722 if (matchedImagePointIndices != nullptr)
723 {
724 matchedImagePointIndices->clear();
725 }
726
727 if (worker != nullptr)
728 {
729 Lock lock;
730 worker->executeFunction(Worker::Function::createStatic(&PoseEstimationT::determineGuidedMatchingsSubset<TImagePointDescriptor, TObjectPointDescriptor, TDescriptorDistance, tImageObjectDistanceFunction>, &camera, &world_T_camera, imagePoints, imagePointDescriptors, objectPoints, &objectPointOctree, objectPointIds, &objectPointDescriptorMap, &matchedImagePoints, &matchedObjectPoints, matchedImagePointIndices, matchedObjectPointIds, maximalDescriptorDistance, &lock, 0u, 0u), 0u, (unsigned int)(numberImagePoints));
731 }
732 else
733 {
734 determineGuidedMatchingsSubset<TImagePointDescriptor, TObjectPointDescriptor, TDescriptorDistance, tImageObjectDistanceFunction>(&camera, &world_T_camera, imagePoints, imagePointDescriptors, objectPoints, &objectPointOctree, objectPointIds, &objectPointDescriptorMap, &matchedImagePoints, &matchedObjectPoints, matchedImagePointIndices, matchedObjectPointIds, maximalDescriptorDistance, nullptr, 0u, (unsigned int)(numberImagePoints));
735 }
736}
737
738template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
739std::vector<TDescriptorDistance> PoseEstimationT::determineDescriptorDistances(const TObjectPointDescriptor* objectPointDescriptors, const TImagePointDescriptor* imagePointDescriptors, const Index32* objectPointDescriptorIndices, const Index32* imagePointDescriptorIndices, const size_t numberCorrespondences)
740{
741 ocean_assert(objectPointDescriptors != nullptr);
742 ocean_assert(imagePointDescriptors != nullptr);
743 ocean_assert(objectPointDescriptorIndices != nullptr);
744 ocean_assert(imagePointDescriptorIndices != nullptr);
745 ocean_assert(numberCorrespondences >= 1);
746
747 std::vector<TDescriptorDistance> result;
748 result.reserve(numberCorrespondences);
749
750 for (size_t n = 0; n < numberCorrespondences; ++n)
751 {
752 const Index32& objectPointDescriptorIndex = objectPointDescriptorIndices[n];
753 const Index32& imagePointDescriptorIndex = imagePointDescriptorIndices[n];
754
755 result.emplace_back(tImageObjectDistanceFunction(imagePointDescriptors[imagePointDescriptorIndex], objectPointDescriptors[objectPointDescriptorIndex]));
756 }
757
758 return result;
759}
760
761template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
762void PoseEstimationT::determineGuidedMatchingsSubset(const AnyCamera* camera, const HomogenousMatrix4* world_T_camera, const Vector2* imagePoints, const TImagePointDescriptor* imagePointDescriptors, const Vector3* objectPoints, const Geometry::Octree* objectPointOctree, const Index32* objectPointIds, const UnorderedDescriptorMap<TObjectPointDescriptor>* objectPointDescriptorMap, Vectors2* matchedImagePoints, Vectors3* matchedObjectPoints, Indices32* matchedImagePointIndices, Indices32* matchedObjectPointIds, const TDescriptorDistance maximalDescriptorDistance, Lock* lock, const unsigned int firstImagePoint, const unsigned int numberImagePoints)
763{
764 static_assert(tImageObjectDistanceFunction != nullptr);
765
766 ocean_assert(camera != nullptr && camera->isValid());
767 ocean_assert(world_T_camera && world_T_camera->isValid());
768
769 ocean_assert(imagePoints != nullptr && imagePointDescriptors != nullptr && objectPoints != nullptr);
770 ocean_assert(objectPointOctree != nullptr && objectPointIds != nullptr && objectPointDescriptorMap != nullptr);
771
772 ocean_assert(matchedImagePoints != nullptr);
773 ocean_assert(matchedObjectPoints != nullptr);
774
775 Vectors2 localMatchedImagePoints;
776 Vectors3 localMatchedObjectPoints;
777 Indices32 localMatchedImagePointIndices;
778 Indices32 localMatchedObjectPointIds;
779
780 localMatchedImagePoints.reserve(numberImagePoints);
781 localMatchedObjectPoints.reserve(numberImagePoints);
782 localMatchedImagePointIndices.reserve(numberImagePoints);
783 localMatchedObjectPointIds.reserve(numberImagePoints);
784
785 const HomogenousMatrix4 flippedCamera_T_world(PinholeCamera::standard2InvertedFlipped(*world_T_camera));
786
787 const Scalar tanHalfAngle = Numeric::tan(Numeric::deg2rad(Scalar(0.2)));
788
789 std::vector<const Indices32*> leaves;
790 leaves.reserve(32);
791
792 constexpr Scalar maximalSqrProjectionError = Scalar(20 * 20);
793
794 Geometry::Octree::ReusableData reusableData;
795
796 for (unsigned int nImagePoint = firstImagePoint; nImagePoint < firstImagePoint + numberImagePoints; ++nImagePoint)
797 {
798 const Vector2& imagePoint = imagePoints[nImagePoint];
799 ocean_assert(camera->isInside(imagePoint));
800
801 const TImagePointDescriptor& imagePointDescriptor = imagePointDescriptors[nImagePoint];
802
803 const Line3 ray = camera->ray(imagePoint, *world_T_camera);
804
805 leaves.clear();
806 objectPointOctree->intersectingLeaves(ray, tanHalfAngle, leaves, reusableData);
807
808 TDescriptorDistance bestDistance = NumericT<TDescriptorDistance>::maxValue();
809 Index32 bestObjectPointIndex = Index32(-1);
810
811 for (const Indices32* leaf : leaves)
812 {
813 for (const Index32& objectPointIndex : *leaf)
814 {
815 const Vector3& objectPoint = objectPoints[objectPointIndex];
816
817 if (PinholeCamera::isObjectPointInFrontIF(flippedCamera_T_world, objectPoint) && camera->projectToImageIF(flippedCamera_T_world, objectPoint).sqrDistance(imagePoint) <= maximalSqrProjectionError)
818 {
819 const Index32 objectPointId = objectPointIds[objectPointIndex];
820 const typename UnorderedDescriptorMap<TObjectPointDescriptor>::const_iterator iObjectPointDescriptor = objectPointDescriptorMap->find(objectPointId);
821 ocean_assert(iObjectPointDescriptor != objectPointDescriptorMap->cend());
822
823 const TDescriptorDistance distance = tImageObjectDistanceFunction(imagePointDescriptor, iObjectPointDescriptor->second);
824
825 if (distance < bestDistance)
826 {
827 bestDistance = distance;
828 bestObjectPointIndex = objectPointIndex;
829 }
830 }
831 }
832 }
833
834 if (bestDistance <= maximalDescriptorDistance)
835 {
836 ocean_assert(bestObjectPointIndex != Index32(-1));
837
838 localMatchedImagePoints.emplace_back(imagePoint);
839 localMatchedObjectPoints.emplace_back(objectPoints[bestObjectPointIndex]);
840
841 localMatchedImagePointIndices.emplace_back(nImagePoint);
842 localMatchedObjectPointIds.emplace_back(objectPointIds[bestObjectPointIndex]);
843 }
844 }
845
846 const OptionalScopedLock scopedLock(lock);
847
848 matchedImagePoints->insert(matchedImagePoints->cend(), localMatchedImagePoints.cbegin(), localMatchedImagePoints.cend());
849 matchedObjectPoints->insert(matchedObjectPoints->cend(), localMatchedObjectPoints.cbegin(), localMatchedObjectPoints.cend());
850
851 if (matchedImagePointIndices != nullptr)
852 {
853 matchedImagePointIndices->insert(matchedImagePointIndices->cend(), localMatchedImagePointIndices.cbegin(), localMatchedImagePointIndices.cend());
854 }
855
856 if (matchedObjectPointIds != nullptr)
857 {
858 matchedObjectPointIds->insert(matchedObjectPointIds->cend(), localMatchedObjectPointIds.cbegin(), localMatchedObjectPointIds.cend());
859 }
860}
861
862}
863
864}
865
866#endif // META_OCEAN_TRACKING_POSE_ESTIMATION_T_H
Ocean::AnyCameraT
This class implements the abstract base class for all AnyCamera objects.
Definition AnyCamera.h:130
Ocean::AnyCameraT::projectToImageIF
virtual VectorT2< T > projectToImageIF(const VectorT3< T > &objectPoint) const =0
Projects a 3D object point into the camera frame.
Ocean::AnyCameraT::isValid
virtual bool isValid() const =0
Returns whether this camera is valid.
Ocean::AnyCameraT::ray
virtual LineT3< T > ray(const VectorT2< T > &distortedImagePoint, const HomogenousMatrixT4< T > &world_T_camera) const =0
Returns a ray starting at the camera's center and intersecting a given 2D point in the image.
Ocean::AnyCameraT::isInside
virtual bool isInside(const VectorT2< T > &imagePoint, const T signedBorder=T(0)) const =0
Returns whether a given 2D image point lies inside the camera frame.
Ocean::Caller< void >::createStatic
static Caller< void > createStatic(typename StaticFunctionPointerMaker< void, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass, NullClass >::Type function)
Creates a new caller container for a static function with no function parameter.
Definition Caller.h:2877
Ocean::CameraT::standard2InvertedFlipped
static HomogenousMatrixT4< U > standard2InvertedFlipped(const HomogenousMatrixT4< U > &world_T_camera)
Transforms a standard homogenous 4x4 viewing (extrinsic camera) matrix into an inverted and flipped c...
Definition Camera.h:734
Ocean::CameraT::isObjectPointInFrontIF
static bool isObjectPointInFrontIF(const HomogenousMatrixT4< T > &flippedCamera_T_world, const VectorT3< T > &objectPoint, const T epsilon=NumericT< T >::eps())
Determines whether a given 3D object point lies in front of a camera while the location of the camera...
Definition Camera.h:811
Ocean::ConstArrayAccessor
This class implements an accessor providing direct access to a constant array of elements.
Definition Accessor.h:400
Ocean::Geometry::RANSAC::iterations
static unsigned int iterations(const unsigned int model, const Scalar successProbability=Scalar(0.99), const Scalar faultyRate=Scalar(0.2), const unsigned int maximalIterations=1000000u)
Calculates the number of iterations necessary to find an outlier-free model data set.
Ocean::Geometry::RANSAC::p3p
static bool p3p(const AnyCamera &anyCamera, const ConstIndexedAccessor< ObjectPoint > &objectPointAccessor, const ConstIndexedAccessor< ImagePoint > &imagePointAccessor, RandomGenerator &randomGenerator, HomogenousMatrix4 &world_T_camera, const unsigned int minimalValidCorrespondences=5u, const bool refine=true, const unsigned int iterations=20u, const Scalar sqrPixelErrorThreshold=Scalar(5 *5), Indices32 *usedIndices=nullptr, Scalar *sqrAccuracy=nullptr, const GravityConstraints *gravityConstraints=nullptr)
Calculates a pose using the perspective pose problem with three point correspondences using any camer...
Ocean::HomogenousMatrixT4::toNull
void toNull()
Sets the matrix to a zero matrix (including the lower right element).
Definition HomogenousMatrix4.h:1795
Ocean::HomogenousMatrixT4::isValid
bool isValid() const
Returns whether this matrix is a valid homogeneous transformation.
Definition HomogenousMatrix4.h:1806
Ocean::LineT3
This class implements an infinite line in 3D space.
Definition Line3.h:68
Ocean::Lock
This class implements a recursive lock object.
Definition Lock.h:31
Ocean::NumericT::deg2rad
static constexpr T deg2rad(const T deg)
Converts deg to rad.
Definition Numeric.h:3235
Ocean::NumericT::tan
static T tan(const T value)
Returns the tangent of a given value.
Definition Numeric.h:1600
Ocean::NumericT::sqr
static constexpr T sqr(const T value)
Returns the square of a given value.
Definition Numeric.h:1495
Ocean::NumericT::maxValue
static constexpr T maxValue()
Returns the max scalar value.
Definition Numeric.h:3247
Ocean::OptionalScopedLock
This class implements an optional recursive scoped lock object locking the lock object only if it's d...
Definition Lock.h:337
Ocean::RandomGenerator
This class implements a generator for random numbers.
Definition RandomGenerator.h:42
Ocean::Tracking::PoseEstimationT
This class implements functions to estimate camera poses using template-based data types.
Definition PoseEstimationT.h:35
Ocean::Tracking::PoseEstimationT::determineGuidedBruteForceMatchings
static void determineGuidedBruteForceMatchings(const AnyCamera &camera, const HomogenousMatrix4 &world_T_roughCamera, const Vector3 *objectPoints, const TObjectPointDescriptor *objectPointDescriptors, const size_t numberObjectPoints, const Vector2 *imagePoints, const TImagePointDescriptor *imagePointDescriptors, const size_t numberImagePoints, TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, Index32 *objectPointDescriptorIndices, Worker *worker)
Determines the guided matching (without known rough camera pose) between image point and object point...
Definition PoseEstimationT.h:594
Ocean::Tracking::PoseEstimationT::determineGuidedBruteForceMatchingsSubset
static void determineGuidedBruteForceMatchingsSubset(const AnyCamera *camera, const HomogenousMatrix4 *world_T_roughCamera, const Vector3 *objectPoints, const TObjectPointDescriptor *objectPointDescriptors, const size_t numberObjectPoints, const Vector2 *imagePoints, const TImagePointDescriptor *imagePointDescriptors, TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, Index32 *objectPointDescriptorIndices, const unsigned int firstImagePoint, const unsigned int numberImagePoints)
Determines the guided matching (without known rough camera pose) between a subset of image point and ...
Definition PoseEstimationT.h:656
Ocean::Tracking::PoseEstimationT::determineGuidedMatchingsSubset
static void determineGuidedMatchingsSubset(const AnyCamera *camera, const HomogenousMatrix4 *world_T_camera, const Vector2 *imagePoints, const TImagePointDescriptor *imagePointDescriptors, const Vector3 *objectPoints, const Geometry::Octree *objectPointOctree, const Index32 *objectPointIds, const UnorderedDescriptorMap< TObjectPointDescriptor > *objectPointDescriptorMap, Vectors2 *matchedImagePoints, Vectors3 *matchedObjectPoints, Indices32 *matchedImagePointIndices, Indices32 *matchedObjectPointIds, const TDescriptorDistance maximalDescriptorDistance, Lock *lock, const unsigned int firstImagePoint, const unsigned int numberImagePoints)
Determines the guided matching between 2D/3D correspondences for a subset of the given 2D image point...
Definition PoseEstimationT.h:762
Ocean::Tracking::PoseEstimationT::determineDescriptorDistances
static std::vector< TDescriptorDistance > determineDescriptorDistances(const TObjectPointDescriptor *objectPointDescriptors, const TImagePointDescriptor *imagePointDescriptors, const Index32 *objectPointDescriptorIndices, const Index32 *imagePointDescriptorIndices, const size_t numberCorrespondences)
Returns the individual descriptor distances for a given set of corresponding feature descriptor pairs...
Definition PoseEstimationT.h:739
Ocean::Tracking::PoseEstimationT::UnorderedDescriptorMap
std::unordered_map< Index32, TDescriptor > UnorderedDescriptorMap
Definition of an unordered map mapping object point ids to descriptors.
Definition PoseEstimationT.h:42
Ocean::Tracking::PoseEstimationT::determineUnguidedBruteForceMatchings
static void determineUnguidedBruteForceMatchings(const TObjectPointDescriptor *objectPointDescriptors, const size_t numberObjectPointDescriptors, const TImagePointDescriptor *imagePointDescriptors, const size_t numberImagePointDescriptors, TDescriptorDistance maximalDescriptorDistance, Index32 *objectPointDescriptorIndices, Worker *worker, TDescriptorDistance *distances=nullptr)
Determines the unguided matching (without apriori information) between image point and object point f...
Definition PoseEstimationT.h:577
Ocean::Tracking::PoseEstimationT::determinePoseBruteForce
static bool determinePoseBruteForce(const AnyCamera &camera, const Vector3 *objectPoints, const TObjectPointDescriptor *objectPointDescriptors, const size_t numberObjectPoints, const Vector2 *imagePoints, const TImagePointDescriptor *imagePointDescriptors, const size_t numberImagePoints, RandomGenerator &randomGenerator, HomogenousMatrix4 &world_T_camera, const unsigned int minimalNumberCorrespondences, const TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, const Scalar inlierRate=Scalar(0.15), Indices32 *usedObjectPointIndices=nullptr, Indices32 *usedImagePointIndices=nullptr, const HomogenousMatrix4 &world_T_roughCamera=HomogenousMatrix4(false), Worker *worker=nullptr)
Determines the 6-DOF pose for 2D/3D correspondences applying a brute-force search.
Definition PoseEstimationT.h:258
Ocean::Tracking::PoseEstimationT::determineUnguidedBruteForceMatchingsSubset
static void determineUnguidedBruteForceMatchingsSubset(const TObjectPointDescriptor *objectPointDescriptors, const size_t numberObjectPointDescriptors, const TImagePointDescriptor *imagePointDescriptors, TDescriptorDistance maximalDescriptorDistance, Index32 *objectPointDescriptorIndices, TDescriptorDistance *distances, const unsigned int firstImagePointDescriptor, const unsigned int numberImagePointDescriptors)
Determines the unguided matching between a subset of image point and object point features applying a...
Definition PoseEstimationT.h:616
Ocean::Tracking::PoseEstimationT::determinePoseBruteForceWithArbitraryDescriptorOrder
static bool determinePoseBruteForceWithArbitraryDescriptorOrder(const AnyCamera &camera, const Vector3 *objectPoints, const TObjectPointDescriptor *objectPointDescriptors, const Index32 *objectPointIndices, const size_t numberObjectDescriptors, const Vector2 *imagePoints, const TImagePointDescriptor *imagePointDescriptors, const size_t numberImagePoints, RandomGenerator &randomGenerator, HomogenousMatrix4 &world_T_camera, const unsigned int minimalNumberCorrespondences, const TDescriptorDistance maximalDescriptorDistance, const Scalar maximalProjectionError, const Scalar inlierRate=Scalar(0.15), Indices32 *usedObjectPointIndices=nullptr, Indices32 *usedImagePointIndices=nullptr, const HomogenousMatrix4 &world_T_roughCamera=HomogenousMatrix4(false))
Determines the 6-DOF pose for 2D/3D correspondences applying a brute-force search.
Definition PoseEstimationT.h:399
Ocean::Tracking::PoseEstimationT::determineGuidedMatchings
static void determineGuidedMatchings(const AnyCamera &camera, const HomogenousMatrix4 &world_T_camera, const Vector2 *imagePoints, const TImagePointDescriptor *imagePointDescriptors, const size_t numberImagePoints, const Vector3 *objectPoints, const Geometry::Octree &objectPointOctree, const Index32 *objectPointIds, const UnorderedDescriptorMap< TObjectPointDescriptor > &objectPointDescriptorMap, Vectors2 &matchedImagePoints, Vectors3 &matchedObjectPoints, const TDescriptorDistance maximalDescriptorDistance, Indices32 *matchedImagePointIndices=nullptr, Indices32 *matchedObjectPointIds=nullptr, Worker *worker=nullptr)
Determines the guided matching between 2D/3D correspondences.
Definition PoseEstimationT.h:708
Ocean::Worker
This class implements a worker able to distribute function calls over different threads.
Definition Worker.h:33
Ocean::Worker::executeFunction
bool executeFunction(const Function &function, const unsigned int first, const unsigned int size, const unsigned int firstIndex=(unsigned int)(-1), const unsigned int sizeIndex=(unsigned int)(-1), const unsigned int minimalIterations=1u, const unsigned int threadIndex=(unsigned int)(-1))
Executes a callback function separable by two function parameters.
size_t
Ocean::Indices32
std::vector< Index32 > Indices32
Definition of a vector holding 32 bit index values.
Definition Base.h:96
Ocean::Index32
uint32_t Index32
Definition of a 32 bit index value.
Definition Base.h:84
Ocean::Geometry::Octree
OctreeT< Scalar > Octree
Definition of an Octree using Scalar as data type.
Definition Octree.h:32
Ocean::Scalar
float Scalar
Definition of a scalar type.
Definition Math.h:129
Ocean::Vectors2
std::vector< Vector2 > Vectors2
Definition of a vector holding Vector2 objects.
Definition Vector2.h:64
Ocean::Vectors3
std::vector< Vector3 > Vectors3
Definition of a vector holding Vector3 objects.
Definition Vector3.h:65
Ocean
The namespace covering the entire Ocean framework.
Definition Accessor.h:15