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