Blender  V3.3
euclidean_resection_test.cc
Go to the documentation of this file.
1 // Copyright (c) 2009 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20 
22 #include "libmv/logging/logging.h"
24 #include "libmv/numeric/numeric.h"
25 #include "testing/testing.h"
26 
27 using namespace libmv::euclidean_resection;
28 using namespace libmv;
29 
30 // Generates all necessary inputs and expected outputs for EuclideanResection.
31 static void CreateCameraSystem(const Mat3& KK,
32  const Mat3X& x_image,
33  const Vec& X_distances,
34  const Mat3& R_input,
35  const Vec3& T_input,
36  Mat2X* x_camera,
37  Mat3X* X_world,
38  Mat3* R_expected,
39  Vec3* T_expected) {
40  int num_points = x_image.cols();
41 
42  Mat3X x_unit_cam(3, num_points);
43  x_unit_cam = KK.inverse() * x_image;
44 
45  // Create normalized camera coordinates to be used as an input to the PnP
46  // function, instead of using NormalizeColumnVectors(&x_unit_cam).
47  *x_camera = x_unit_cam.block(0, 0, 2, num_points);
48  for (int i = 0; i < num_points; ++i) {
49  x_unit_cam.col(i).normalize();
50  }
51 
52  // Create the 3D points in the camera system.
53  Mat X_camera(3, num_points);
54  for (int i = 0; i < num_points; ++i) {
55  X_camera.col(i) = X_distances(i) * x_unit_cam.col(i);
56  }
57 
58  // Apply the transformation to the camera 3D points
59  Mat translation_matrix(3, num_points);
60  translation_matrix.row(0).setConstant(T_input(0));
61  translation_matrix.row(1).setConstant(T_input(1));
62  translation_matrix.row(2).setConstant(T_input(2));
63 
64  *X_world = R_input * X_camera + translation_matrix;
65 
66  // Create the expected result for comparison.
67  *R_expected = R_input.transpose();
68  *T_expected = *R_expected * (-T_input);
69 };
70 
71 TEST(AbsoluteOrientation, QuaternionSolution) {
72  int num_points = 4;
73  Mat X;
74  Mat Xp;
75  X = 100 * Mat::Random(3, num_points);
76 
77  // Create a random translation and rotation.
78  Mat3 R_input;
79  R_input = Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ()) *
80  Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitY()) *
81  Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ());
82 
83  Vec3 t_input;
84  t_input.setRandom();
85  t_input = 100 * t_input;
86 
87  Mat translation_matrix(3, num_points);
88  translation_matrix.row(0).setConstant(t_input(0));
89  translation_matrix.row(1).setConstant(t_input(1));
90  translation_matrix.row(2).setConstant(t_input(2));
91 
92  // Create the transformed 3D points Xp as Xp = R * X + t.
93  Xp = R_input * X + translation_matrix;
94 
95  // Output variables.
96  Mat3 R;
97  Vec3 t;
98 
99  AbsoluteOrientation(X, Xp, &R, &t);
100 
101  EXPECT_MATRIX_NEAR(t, t_input, 1e-6);
102  EXPECT_MATRIX_NEAR(R, R_input, 1e-8);
103 }
104 
105 TEST(EuclideanResection, Points4KnownImagePointsRandomTranslationRotation) {
106  // In this test only the translation and rotation are random. The image
107  // points are selected from a real case and are well conditioned.
108  Vec2i image_dimensions;
109  image_dimensions << 1600, 1200;
110 
111  Mat3 KK;
112  // clang-format off
113  KK << 2796, 0, 804,
114  0 , 2796, 641,
115  0, 0, 1;
116  // clang-format on
117 
118  // The real image points.
119  int num_points = 4;
120  Mat3X x_image(3, num_points);
121  // clang-format off
122  x_image << 1164.06, 734.948, 749.599, 430.727,
123  681.386, 844.59, 496.315, 580.775,
124  1, 1, 1, 1;
125  // clang-format on
126 
127  // A vector of the 4 distances to the 3D points.
128  Vec X_distances = 100 * Vec::Random(num_points).array().abs();
129 
130  // Create the random camera motion R and t that resection should recover.
131  Mat3 R_input;
132  R_input = Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ()) *
133  Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitY()) *
134  Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ());
135 
136  Vec3 T_input;
137  T_input.setRandom();
138  T_input = 100 * T_input;
139 
140  // Create the camera system, also getting the expected result of the
141  // transformation.
142  Mat3 R_expected;
143  Vec3 T_expected;
144  Mat3X X_world;
145  Mat2X x_camera;
147  x_image,
148  X_distances,
149  R_input,
150  T_input,
151  &x_camera,
152  &X_world,
153  &R_expected,
154  &T_expected);
155 
156  // Finally, run the code under test.
157  Mat3 R_output;
158  Vec3 T_output;
160  x_camera, X_world, &R_output, &T_output, RESECTION_ANSAR_DANIILIDIS);
161 
162  EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5);
163  EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7);
164 
165  // For now, the EPnP doesn't have a non-linear optimization step and so is
166  // not precise enough with only 4 points.
167  //
168  // TODO(jmichot): Reenable this test when there is nonlinear refinement.
169 #if 0
170  R_output.setIdentity();
171  T_output.setZero();
172 
173  EuclideanResection(x_camera, X_world,
174  &R_output, &T_output,
176 
177  EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5);
178  EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7);*/
179 #endif
180 }
181 
182 // TODO(jmichot): Reduce the code duplication here with the code above.
183 TEST(EuclideanResection, Points6AllRandomInput) {
184  Mat3 KK;
185  // clang-format off
186  KK << 2796, 0, 804,
187  0 , 2796, 641,
188  0, 0, 1;
189  // clang-format on
190 
191  // Create random image points for a 1600x1200 image.
192  int w = 1600;
193  int h = 1200;
194  int num_points = 6;
195  Mat3X x_image(3, num_points);
196  x_image.row(0) = w * Vec::Random(num_points).array().abs();
197  x_image.row(1) = h * Vec::Random(num_points).array().abs();
198  x_image.row(2).setOnes();
199 
200  // Normalized camera coordinates to be used as an input to the PnP function.
201  Mat2X x_camera;
202  Vec X_distances = 100 * Vec::Random(num_points).array().abs();
203 
204  // Create the random camera motion R and t that resection should recover.
205  Mat3 R_input;
206  R_input = Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ()) *
207  Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitY()) *
208  Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ());
209 
210  Vec3 T_input;
211  T_input.setRandom();
212  T_input = 100 * T_input;
213 
214  // Create the camera system.
215  Mat3 R_expected;
216  Vec3 T_expected;
217  Mat3X X_world;
219  x_image,
220  X_distances,
221  R_input,
222  T_input,
223  &x_camera,
224  &X_world,
225  &R_expected,
226  &T_expected);
227 
228  // Test each of the resection methods.
229  {
230  Mat3 R_output;
231  Vec3 T_output;
233  x_camera, X_world, &R_output, &T_output, RESECTION_ANSAR_DANIILIDIS);
234  EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5);
235  EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7);
236  }
237  {
238  Mat3 R_output;
239  Vec3 T_output;
240  EuclideanResection(x_camera, X_world, &R_output, &T_output, RESECTION_EPNP);
241  EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5);
242  EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7);
243  }
244  {
245  Mat3 R_output;
246  Vec3 T_output;
247  EuclideanResection(x_image, X_world, KK, &R_output, &T_output);
248  EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5);
249  EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7);
250  }
251 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble t
#define X
Definition: GeomUtils.cpp:199
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
static void CreateCameraSystem(const Mat3 &KK, const Mat3X &x_image, const Vec &X_distances, const Mat3 &R_input, const Vec3 &T_input, Mat2X *x_camera, Mat3X *X_world, Mat3 *R_expected, Vec3 *T_expected)
float[3][3] Mat3
Definition: gpu_matrix.cc:27
#define R
bool EuclideanResection(const Mat2X &x_camera, const Mat3X &X_world, Mat3 *R, Vec3 *t, ResectionMethod method)
void AbsoluteOrientation(const Mat3X &X, const Mat3X &Xp, Mat3 *R, Vec3 *t)
Eigen::VectorXd Vec
Definition: numeric.h:61
TEST(PolynomialCameraIntrinsics2, ApplyOnFocalCenter)
Eigen::MatrixXd Mat
Definition: numeric.h:60
Eigen::Vector3d Vec3
Definition: numeric.h:106
Eigen::Matrix< double, 3, Eigen::Dynamic > Mat3X
Definition: numeric.h:92
Eigen::Vector2i Vec2i
Definition: numeric.h:131
Eigen::Matrix< double, 2, Eigen::Dynamic > Mat2X
Definition: numeric.h:91