Ocean
Loading...
Searching...
No Matches
P3P.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_P3P_H
9#define META_OCEAN_GEOMETRY_P3P_H
10
13
16
17namespace Ocean
18{
19
20namespace Geometry
21{
22
23/**
24 * This class provides functions determining the camera's pose by a set of three 3D object and 2D image point correspondences.
25 * @ingroup geometry
26 */
27class OCEAN_GEOMETRY_EXPORT P3P : public PerspectivePose
28{
29 public:
30
31 /**
32 * Calculates the possible camera poses for three correspondences between 3D object points and 2D image points.
33 * The 3D object points as well as the resulting camera poses are defined in relation to a common world coordinate system.<br>
34 * Each pose is defined using a default camera pointing into the negative z-space of the coordinate system, with x-axis to the right of the camera frame, and y-axis pointing upwards (this coordinate system is often used in Computer Graphics).<br>
35 * The resulting poses can be transformed to an inverted flipped coordinate system e.g., by using PinholeCamera::standard2InvertedFlipped().<br>
36 * An inverted and flipped pose is pointing into the positive z-space of the coordinate system, with x-axis to the right of the camera frame, and y-axis pointing downwards (this coordinate system is often used in Computer Vision).<br>
37 * The provided image points should be defined in the domain of a normal image (with origin in the upper left corner, x pointing to the right, y pointing downwards).<br>
38 * The p3p can result in at most four different poses due to the under-determined system of equations.
39 * @param anyCamera The camera profile defining the projection, must be valid
40 * @param objectPoints Three 3D objects points each corresponding to a different 2D image point, the points must not be collinear
41 * @param imagePoints Three 2D image points each corresponding to a different 3D object point
42 * @param world_T_cameras The resulting transformation matrices receiving the different poses, the buffer must be large enough to store up to four resulting poses
43 * @return Resulting number of different poses, with range [0, 4]
44 * @tparam TCamera The data type of the camera profile, either 'float' or 'double'
45 * @tparam TPoint The data type image and object points, either 'float' or 'double'
46 * @see PinholeCamera::standard2InvertedFlipped(), PinholeCamera::invertedFlipped2Standard(), PinholeCamera::undistort().
47 */
48 template <typename TCamera, typename TPoint>
49 static unsigned int poses(const AnyCameraT<TCamera>& anyCamera, const VectorT3<TPoint>* objectPoints, const VectorT2<TPoint>* imagePoints, HomogenousMatrixT4<TPoint>* world_T_cameras);
50
51 /**
52 * Calculates the possible camera poses for three correspondences between 3D object points and 3D rays starting at the camera's center of projection and pointing towards the 3D object points.
53 * The 3D object points are defined in relation to a world coordinate system, while the 3D rays are defined in relation to the coordinate system of the camera.<br>
54 * Each pose is defined using a default camera pointing into the negative z-space of the coordinate system, with x-axis to the right of the camera frame and y-axis pointing upwards.<br>
55 * The resulting poses can be transformed to an inverted flipped coordinate system e.g., by using PinholeCamera::standard2InvertedFlipped().<br>
56 * The p3p can result in at most four different poses due to the under-determined system of equations.<br>
57 * @param objectPoints Three 3D objects points each corresponding to a different 2D image point, the points must not be collinear
58 * @param imageRays Three 3D rays with unit length, defined in the coordinate system of the camera, starting at the camera's center of projection (equal to the origin), hitting the image plane at image points corresponding to the 3D object points and pointing towards the object points, each ray corresponds to one 3D object point
59 * @param cameraPoses The resulting transformation matrices receiving the different poses, the buffer must be large enough to store up to four resulting poses
60 * @return Resulting number of different poses, with range [0, 4]
61 * @tparam T Data type of e.g., the vector elements to be used, either 'float' or 'double'
62 * @see PinholeCamera::standard2InvertedFlipped(), PinholeCamera::invertedFlipped2Standard(), PinholeCamera::undistort().
63 */
64 template <typename T>
65 static unsigned int poses(const VectorT3<T>* objectPoints, const VectorT3<T>* imageRays, HomogenousMatrixT4<T>* cameraPoses);
66
67 protected:
68
69 /**
70 * Constructs the closest point on the line between two object points and the camera's projection center.
71 * @param objectPoint0 First object point intersecting the line
72 * @param objectPoint1 Second object point intersecting the line
73 * @param objectDistance01 Distance between first and second object point
74 * @param objectDistanceToCP0 Distance between first object point and the camera's projection center
75 * @param objectDistanceToCP1 Distance between second object point and the camera's projection center
76 * @return Resulting closest point
77 * @tparam T Data type of e.g., the vector elements to be used, either 'float' or 'double'
78 */
79 template <typename T>
80 static inline VectorT3<T> constructClosestPointToCP(const VectorT3<T>& objectPoint0, const VectorT3<T>& objectPoint1, const T objectDistance01, const T objectDistanceToCP0, const T objectDistanceToCP1);
81
82 /**
83 * Constructs the closest point to the camera's projection center lying on the object surface triangle.
84 * @param objectPoint0 First object point intersecting the object surface triangle
85 * @param objectPoint1 Second object point intersecting the object surface triangle
86 * @param objectPoint2 Third object point intersecting the object surface triangle
87 * @param closestPoint01 Closest point to the camera's projection center lying on the line between first and second object point
88 * @param closestPoint02 Closest point to the camera's projection center lying on the line between first and third object point
89 * @param point Resulting closes point
90 * @return True, if succeeded
91 * @tparam T Data type of e.g., the vector elements to be used, either 'float' or 'double'
92 */
93 template <typename T>
94 static inline bool constructClosestPointToCPOnObjectPlane(const VectorT3<T>& objectPoint0, const VectorT3<T>& objectPoint1, const VectorT3<T>& objectPoint2, const VectorT3<T>& closestPoint01, const VectorT3<T>& closestPoint02, VectorT3<T>& point);
95
96 /**
97 * Returns the square of a value.
98 * @param value The value to square
99 * @return The squared value
100 * @tparam T Data type of e.g., the vector elements to be used, either 'float' or 'double'
101 */
102 template <typename T>
103 static inline T sqr(const T value);
104};
105
106template <typename T>
107inline VectorT3<T> P3P::constructClosestPointToCP(const VectorT3<T>& objectPoint0, const VectorT3<T>& objectPoint1, const T objectDistance01, const T objectDistanceToCP0, const T objectDistanceToCP1)
108{
109 ocean_assert(NumericT<T>::isNotEqualEps(objectDistance01));
110
111 /**
112 * We combine the following steps:
113 * const Scalar cos_ObjectPoint0 = (objectDistanceToCP1 * objectDistanceToCP1 - objectDistanceToCP0 * objectDistanceToCP0 - objectDistance01 * objectDistance01) / (-2 * objectDistanceToCP0 * objectDistance01);
114 * const Scalar distanceToClosesPoint = cos_ObjectPoint0 * objectDistanceToCP0;
115 * const Vector3 directionObjectPoint0To1 = (objectPoint1 - objectPoint0) / objectDistance01;
116 * return objectPoint0 + directionObjectPoint0To1 * distanceToClosesPoint;
117 */
118
119 const T factor = T(-0.5) * (objectDistanceToCP1 * objectDistanceToCP1 - objectDistanceToCP0 * objectDistanceToCP0 - objectDistance01 * objectDistance01) / (objectDistance01 * objectDistance01);
120
121 return objectPoint0 * (T(1) - factor) + objectPoint1 * factor;
122}
123
124template <typename T>
125inline bool P3P::constructClosestPointToCPOnObjectPlane(const VectorT3<T>& objectPoint0, const VectorT3<T>& objectPoint1, const VectorT3<T>& objectPoint2, const VectorT3<T>& closestPoint01, const VectorT3<T>& closestPoint02, VectorT3<T>& point)
126{
127 const VectorT3<T> objectDirection01 = objectPoint1 - objectPoint0;
128 const VectorT3<T> objectDirection02 = objectPoint2 - objectPoint0;
129 ocean_assert(NumericT<T>::isNotEqualEps(objectDirection01.length()));
130 ocean_assert(NumericT<T>::isNotEqualEps(objectDirection02.length()));
131
132 const VectorT3<T> objectPlaneNormal = objectDirection01.cross(objectDirection02); // normal not having unit length
133
134 VectorT3<T> d01 = objectPlaneNormal.cross(objectDirection01);
135 VectorT3<T> d02 = objectPlaneNormal.cross(objectDirection02);
136
137 if (!d01.normalize() || !d02.normalize())
138 return false;
139
140 ocean_assert_accuracy(NumericT<T>::isWeakEqualEps(d01 * objectPlaneNormal.normalizedOrZero()));
141 ocean_assert_accuracy(NumericT<T>::isWeakEqualEps(d02 * objectPlaneNormal.normalizedOrZero()));
142
143 ocean_assert_accuracy(NumericT<T>::isWeakEqualEps(d01 * objectDirection01));
144 ocean_assert_accuracy(NumericT<T>::isWeakEqualEps(d02 * objectDirection02));
145
146 const LineT3<T> line01(closestPoint01, d01);
147 const LineT3<T> line02(closestPoint02, d02);
148
149#ifdef OCEAN_INTENSIVE_DEBUG
150 {
151 // we ensure that both lines are located in the plane (and respect that a line point may be very far away from our object points
152 const PlaneT3<T> debugPlane(objectPoint0, objectPoint1, objectPoint2);
153
154 const VectorT3<T> debugDirection0(line01.point() - objectPoint0);
155 ocean_assert(NumericT<T>::isEqualEps(debugDirection0.length()) || NumericT<T>::angleIsEqual(debugPlane.normal().angle(debugDirection0), NumericT<T>::pi_2(), NumericT<T>::deg2rad(T(0.1))));
156
157 const VectorT3<T> debugDirection1(line01.point(1) - objectPoint0);
158 ocean_assert(NumericT<T>::isEqualEps(debugDirection1.length()) || NumericT<T>::angleIsEqual(debugPlane.normal().angle(debugDirection1), NumericT<T>::pi_2(), NumericT<T>::deg2rad(T(0.1))));
159
160 const VectorT3<T> debugDirection2(line02.point(0) - objectPoint0);
161 ocean_assert(NumericT<T>::isEqualEps(debugDirection2.length()) || NumericT<T>::angleIsEqual(debugPlane.normal().angle(debugDirection2), NumericT<T>::pi_2(), NumericT<T>::deg2rad(T(0.1))));
162
163 const VectorT3<T> debugDirection3(line02.point(1) - objectPoint0);
164 ocean_assert(NumericT<T>::isEqualEps(debugDirection3.length()) || NumericT<T>::angleIsEqual(debugPlane.normal().angle(debugDirection3), NumericT<T>::pi_2(), NumericT<T>::deg2rad(T(0.1))));
165 }
166#endif
167
168 VectorT3<T> first, second;
169
170 if (!line01.nearestPoints(line02, first, second))
171 return false;
172
173 point = (first + second) * T(0.5);
174 return true;
175}
176
177template <typename T>
178inline T P3P::sqr(const T value)
179{
180 return value * value;
181}
182
183}
184
185}
186
187#endif // META_OCEAN_GEOMETRY_P3P_H
This class implements the abstract base class for all AnyCamera objects.
Definition AnyCamera.h:130
This class provides functions determining the camera's pose by a set of three 3D object and 2D image ...
Definition P3P.h:28
static T sqr(const T value)
Returns the square of a value.
Definition P3P.h:178
static unsigned int poses(const VectorT3< T > *objectPoints, const VectorT3< T > *imageRays, HomogenousMatrixT4< T > *cameraPoses)
Calculates the possible camera poses for three correspondences between 3D object points and 3D rays s...
static bool constructClosestPointToCPOnObjectPlane(const VectorT3< T > &objectPoint0, const VectorT3< T > &objectPoint1, const VectorT3< T > &objectPoint2, const VectorT3< T > &closestPoint01, const VectorT3< T > &closestPoint02, VectorT3< T > &point)
Constructs the closest point to the camera's projection center lying on the object surface triangle.
Definition P3P.h:125
static unsigned int poses(const AnyCameraT< TCamera > &anyCamera, const VectorT3< TPoint > *objectPoints, const VectorT2< TPoint > *imagePoints, HomogenousMatrixT4< TPoint > *world_T_cameras)
Calculates the possible camera poses for three correspondences between 3D object points and 2D image ...
static VectorT3< T > constructClosestPointToCP(const VectorT3< T > &objectPoint0, const VectorT3< T > &objectPoint1, const T objectDistance01, const T objectDistanceToCP0, const T objectDistanceToCP1)
Constructs the closest point on the line between two object points and the camera's projection center...
Definition P3P.h:107
This class is the base class for all perspective pose classes.
Definition PerspectivePose.h:27
This class implements a 4x4 homogeneous transformation matrix using floating point values with the pr...
Definition HomogenousMatrix4.h:110
This class implements an infinite line in 3D space.
Definition Line3.h:68
bool nearestPoints(const LineT3< T > &line, VectorT3< T > &first, VectorT3< T > &second) const
Returns the two nearest points for two crossing lines.
Definition Line3.h:482
const VectorT3< T > & point() const
Returns a point on the line.
Definition Line3.h:269
This class provides basic numeric functionalities.
Definition Numeric.h:57
This class implements a plane in 3D space.
Definition Plane3.h:73
const VectorT3< T > & normal() const
Returns the normal of the plane.
Definition Plane3.h:340
This class implements a vector with two elements.
Definition Vector2.h:96
This class implements a vector with three elements.
Definition Vector3.h:97
bool normalize()
Normalizes this vector.
Definition Vector3.h:659
VectorT3< T > cross(const VectorT3< T > &vector) const
Returns the cross product of two vectors.
Definition Vector3.h:609
VectorT3< T > normalizedOrZero() const
Returns the normalized vector.
Definition Vector3.h:631
T length() const
Returns the length of the vector.
Definition Vector3.h:676
unsigned int sqr(const char value)
Returns the square value of a given value.
Definition base/Utilities.h:1029
The namespace covering the entire Ocean framework.
Definition Accessor.h:15