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_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
24namespace Ocean
25{
26
27namespace Tracking
28{
29
30namespace MapBuilding
31{
32
33/**
34 * This class implements functions to estimate camera poses using template-based data types.
35 * @ingroup trackingmapbuilding
36 */
37class 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
260template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
261bool 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
403template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
404bool 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
581template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
582void 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
598template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
599void 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
620template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
621void 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
660template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
661void 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
712template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
713void 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
743template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
744std::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
766template <typename TImagePointDescriptor, typename TObjectPointDescriptor, typename TDescriptorDistance, TDescriptorDistance(*tImageObjectDistanceFunction)(const TImagePointDescriptor&, const TObjectPointDescriptor&)>
767void 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::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::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:68
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::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