Ocean
Loading...
Searching...
No Matches
StereoscopicGeometry.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_GEOMETRY_STEREOSCOPIC_GEOMETRY_H
9#define META_OCEAN_GEOMETRY_STEREOSCOPIC_GEOMETRY_H
10
12
13#include "ocean/base/Accessor.h"
15
18
19namespace Ocean
20{
21
22namespace Geometry
23{
24
25/**
26 * This class implements function for stereoscopic geometry.
27 * @ingroup geometry
28 */
29class OCEAN_GEOMETRY_EXPORT StereoscopicGeometry
30{
31 public:
32
33 /**
34 * Determines the pose transformation between two given camera frames from which corresponding image point pairs are given.
35 * For each image point in the first frame a corresponding image point in the second frame must be provided.<br>
36 * Further, this function determines the 3D object points which belong to the given image points.<br>
37 * The first camera pose is expected to be the identity camera pose (a default camera in the origin, pointing towards the negative z-space with y-axis upwards.<br>
38 * The function can support outliers in the given point correspondences (to some extend).
39 * @param camera The camera profile defining the projection, must be valid
40 * @param imagePoints0 The image points located in the first frame, at least 5
41 * @param imagePoints1 The image points located in the second frame, each point must have a corresponding image point in the first frame with same index
42 * @param randomGenerator Random generator object
43 * @param world_T_camera1 The resulting camera pose for the second camera, with a default camera pose pointing towards the negative z-space with y-axis upwards
44 * @param objectPoints Optional resulting 3D locations of the object points which are visible in both camera frames (the image points are the projections of these object points)
45 * @param validIndices Optional resulting indices of the given point correspondences which are valid regarding the defined error thresholds
46 * @param maxRotationalSqrError The maximal squared pixel error between a projected object point and a corresponding image point so that the pair counts as valid for rotational camera motion
47 * @param maxArbitrarySqrError The maximal squared pixel error between a projected object point and a corresponding image point so that the pair counts as valid for arbitrary camera motion
48 * @param iterations The number of iterations that will be applied finding a better pose result
49 * @param rotationalMotionMinimalValidCorrespondencesPercent The minimal number of valid correspondences (defined as percent of the entire number of correspondences) that are necessary so that the camera motion is accepted to be pure rotational, with range [0, 1]
50 * @return True, if succeeded
51 */
52 static bool cameraPose(const AnyCamera& camera, const ConstIndexedAccessor<Vector2>& imagePoints0, const ConstIndexedAccessor<Vector2>& imagePoints1, RandomGenerator& randomGenerator, HomogenousMatrix4& world_T_camera1, Vectors3* objectPoints = nullptr, Indices32* validIndices = nullptr, const Scalar maxRotationalSqrError = Scalar(1.5 * 1.5), const Scalar maxArbitrarySqrError = Scalar(3.5 * 3.5), const unsigned int iterations = 100u, const Scalar rotationalMotionMinimalValidCorrespondencesPercent = Scalar(0.9));
53
54 /**
55 * Determines valid correspondences between 2D image points and 3D camera points for two individual camera frames concurrently.
56 * Beware: The given camera matrices are not equal to a extrinsic matrix.<br>
57 * Instead, the camera matrices are the extrinsic camera matrix flipped around the x-axis and inverted afterwards.<br>
58 * @param camera The camera profile defining the projection, must be valid
59 * @param flippedCamera0_T_world The transformation between world and the flipped first camera (a camera pointing towards the positive z-space with y-axis downwards), must be valid
60 * @param flippedCamera1_T_world The transformation between world and the flipped second camera (a camera pointing towards the positive z-space with y-axis downwards), must be valid
61 * @param objectPoints Accessor providing the 3D object points
62 * @param imagePoints0 Accessor providing the 2D image points for the first camera frame, one image point for each 3D object point
63 * @param imagePoints1 Accessor providing the 2D image points for the second camera frame, on image point for each image point in the first frame (and for each 3D object point)
64 * @param validIndices Resulting indices of all valid correspondences
65 * @param maxSqrError The maximal square pixel error between a projected 3D object point and a corresponding 2D image point to count as valid, with range [0, infinity)
66 * @param onlyFrontObjectPoints True, to accept only object points lying in front of both camera frames
67 * @param totalSqrError Optional resulting sum of all square pixel errors for all valid point correspondences (for both frames)
68 * @param minimalValidCorrespondences Optional number of valid correspondences that must be reached, otherwise the function stops without providing a useful result, with range [0, objectPoints.size()]
69 * @return True, if succeeded; False, if the function stops due to the defined minimal number of thresholds
70 * @tparam TAccessorObjectPoints The template type of the accessor for the object points
71 * @tparam TAccessorImagePoints0 The template type of the accessor for the first image points
72 * @tparam TAccessorImagePoints1 The template type of the accessor for the second image points
73 * @tparam tUseBorderDistortionIfOutside True, to apply the camera distortion from the nearest point lying on the frame border if the point lies outside the visible camera area; False, to apply the distortion from the given position
74 */
75 template <typename TAccessorObjectPoints, typename TAccessorImagePoints0, typename TAccessorImagePoints1, bool tUseBorderDistortionIfOutside>
76 static bool determineValidCorrespondencesIF(const AnyCamera& camera, const HomogenousMatrix4& flippedCamera0_T_world, const HomogenousMatrix4& flippedCamera1_T_world, const TAccessorObjectPoints& objectPoints, const TAccessorImagePoints0& imagePoints0, const TAccessorImagePoints1& imagePoints1, Indices32& validIndices, const Scalar maxSqrError = Scalar(3.5 * 3.5), const bool onlyFrontObjectPoints = true, Scalar* totalSqrError = nullptr, const size_t minimalValidCorrespondences = 0);
77};
78
79template <typename TAccessorObjectPoints, typename TAccessorImagePoints0, typename TAccessorImagePoints1, bool tUseBorderDistortionIfOutside>
80bool StereoscopicGeometry::determineValidCorrespondencesIF(const AnyCamera& camera, const HomogenousMatrix4& flippedCamera0_T_world, const HomogenousMatrix4& flippedCamera1_T_world, const TAccessorObjectPoints& objectPoints, const TAccessorImagePoints0& imagePoints0, const TAccessorImagePoints1& imagePoints1, Indices32& validIndices, const Scalar maxSqrError, const bool onlyFrontObjectPoints, Scalar* totalSqrError, const size_t minimalValidCorrespondences)
81{
82 ocean_assert(camera.isValid());
83 ocean_assert(flippedCamera0_T_world.isValid());
84 ocean_assert(flippedCamera1_T_world.isValid());
85 ocean_assert(objectPoints.size() == imagePoints0.size() && imagePoints0.size() == imagePoints1.size());
86 ocean_assert(maxSqrError >= 0);
87
88 ocean_assert(validIndices.empty());
89 validIndices.clear();
90
91 Scalar error = 0;
92
93 for (size_t n = 0u; n < imagePoints0.size(); ++n)
94 {
95 // stop if we cannot reach a specified number of valid correspondences anymore
96 if (minimalValidCorrespondences != 0 && imagePoints0.size() + validIndices.size() - n < minimalValidCorrespondences)
97 return false;
98
99 if (onlyFrontObjectPoints)
100 {
101 // we do not count this object point if it is located behind at least one camera
102 if (AnyCamera::isObjectPointInFrontIF(flippedCamera0_T_world, objectPoints[n]) == false || AnyCamera::isObjectPointInFrontIF(flippedCamera1_T_world, objectPoints[n]) == false)
103 {
104 continue;
105 }
106 }
107
108 const Scalar sqrDistance0 = camera.projectToImageIF(flippedCamera0_T_world, objectPoints[n]).sqrDistance(imagePoints0[n]);
109 const Scalar sqrDistance1 = camera.projectToImageIF(flippedCamera1_T_world, objectPoints[n]).sqrDistance(imagePoints1[n]);
110
111 if (sqrDistance0 < maxSqrError && sqrDistance1 < maxSqrError)
112 {
113 validIndices.push_back((unsigned int)n);
114 error += sqrDistance0 + sqrDistance1;
115 }
116 }
117
118 if (totalSqrError != nullptr)
119 {
120 *totalSqrError = error;
121 }
122
123 return true;
124}
125
126}
127
128}
129
130#endif // META_OCEAN_GEOMETRY_STEREOSCOPIC_GEOMETRY_H
This class implements the abstract base class for all AnyCamera objects.
Definition AnyCamera.h:130
virtual VectorT2< T > projectToImageIF(const VectorT3< T > &objectPoint) const =0
Projects a 3D object point into the camera frame.
virtual bool isValid() const =0
Returns whether this camera is valid.
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
This class implements a base class for all indexed-based accessors allowing a constant reference acce...
Definition Accessor.h:241
This class implements function for stereoscopic geometry.
Definition StereoscopicGeometry.h:30
static bool cameraPose(const AnyCamera &camera, const ConstIndexedAccessor< Vector2 > &imagePoints0, const ConstIndexedAccessor< Vector2 > &imagePoints1, RandomGenerator &randomGenerator, HomogenousMatrix4 &world_T_camera1, Vectors3 *objectPoints=nullptr, Indices32 *validIndices=nullptr, const Scalar maxRotationalSqrError=Scalar(1.5 *1.5), const Scalar maxArbitrarySqrError=Scalar(3.5 *3.5), const unsigned int iterations=100u, const Scalar rotationalMotionMinimalValidCorrespondencesPercent=Scalar(0.9))
Determines the pose transformation between two given camera frames from which corresponding image poi...
static bool determineValidCorrespondencesIF(const AnyCamera &camera, const HomogenousMatrix4 &flippedCamera0_T_world, const HomogenousMatrix4 &flippedCamera1_T_world, const TAccessorObjectPoints &objectPoints, const TAccessorImagePoints0 &imagePoints0, const TAccessorImagePoints1 &imagePoints1, Indices32 &validIndices, const Scalar maxSqrError=Scalar(3.5 *3.5), const bool onlyFrontObjectPoints=true, Scalar *totalSqrError=nullptr, const size_t minimalValidCorrespondences=0)
Determines valid correspondences between 2D image points and 3D camera points for two individual came...
Definition StereoscopicGeometry.h:80
bool isValid() const
Returns whether this matrix is a valid homogeneous transformation.
Definition HomogenousMatrix4.h:1806
This class implements a generator for random numbers.
Definition RandomGenerator.h:42
std::vector< Index32 > Indices32
Definition of a vector holding 32 bit index values.
Definition Base.h:96
float Scalar
Definition of a scalar type.
Definition Math.h:129
std::vector< Vector3 > Vectors3
Definition of a vector holding Vector3 objects.
Definition Vector3.h:65
The namespace covering the entire Ocean framework.
Definition Accessor.h:15