Blender  V3.3
intersect.cc
Go to the documentation of this file.
1 // Copyright (c) 2011 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 
23 #include "libmv/base/vector.h"
24 #include "libmv/logging/logging.h"
29 #include "libmv/numeric/numeric.h"
32 
33 #include "ceres/ceres.h"
34 
35 namespace libmv {
36 
37 namespace {
38 
39 class EuclideanIntersectCostFunctor {
40  public:
41  EuclideanIntersectCostFunctor(const Marker& marker,
42  const EuclideanCamera& camera)
43  : marker_(marker), camera_(camera) {}
44 
45  template <typename T>
46  bool operator()(const T* X, T* residuals) const {
47  typedef Eigen::Matrix<T, 3, 3> Mat3;
48  typedef Eigen::Matrix<T, 3, 1> Vec3;
49 
50  Vec3 x(X);
51  Mat3 R(camera_.R.cast<T>());
52  Vec3 t(camera_.t.cast<T>());
53 
54  Vec3 projected = R * x + t;
55  projected /= projected(2);
56 
57  residuals[0] = (projected(0) - T(marker_.x)) * marker_.weight;
58  residuals[1] = (projected(1) - T(marker_.y)) * marker_.weight;
59 
60  return true;
61  }
62 
63  const Marker& marker_;
64  const EuclideanCamera& camera_;
65 };
66 
67 } // namespace
68 
71  if (markers.size() < 2) {
72  return false;
73  }
74 
75  // Compute projective camera matrices for the cameras the intersection is
76  // going to use.
77  Mat3 K = Mat3::Identity();
78  vector<Mat34> cameras;
79  Mat34 P;
80  for (int i = 0; i < markers.size(); ++i) {
82  P_From_KRt(K, camera->R, camera->t, &P);
83  cameras.push_back(P);
84  }
85 
86  // Stack the 2D coordinates together as required by NViewTriangulate.
87  Mat2X points(2, markers.size());
88  for (int i = 0; i < markers.size(); ++i) {
89  points(0, i) = markers[i].x;
90  points(1, i) = markers[i].y;
91  }
92 
93  Vec4 Xp;
94  LG << "Intersecting with " << markers.size() << " markers.";
95  NViewTriangulateAlgebraic(points, cameras, &Xp);
96 
97  // Get euclidean version of the homogeneous point.
98  Xp /= Xp(3);
99  Vec3 X = Xp.head<3>();
100 
101  ceres::Problem problem;
102 
103  // Add residual blocks to the problem.
104  int num_residuals = 0;
105  for (int i = 0; i < markers.size(); ++i) {
106  const Marker& marker = markers[i];
107  if (marker.weight != 0.0) {
108  const EuclideanCamera& camera =
110 
111  problem.AddResidualBlock(
112  new ceres::AutoDiffCostFunction<EuclideanIntersectCostFunctor,
113  2, /* num_residuals */
114  3>(
115  new EuclideanIntersectCostFunctor(marker, camera)),
116  NULL,
117  &X(0));
118  num_residuals++;
119  }
120  }
121 
122  // TODO(sergey): Once we'll update Ceres to the next version
123  // we wouldn't need this check anymore -- Ceres will deal with
124  // zero-sized problems nicely.
125  LG << "Number of residuals: " << num_residuals;
126  if (!num_residuals) {
127  LG << "Skipping running minimizer with zero residuals";
128 
129  // We still add 3D point for the track regardless it was
130  // optimized or not. If track is a constant zero it'll use
131  // algebraic intersection result as a 3D coordinate.
132 
133  Vec3 point = X.head<3>();
135 
136  return true;
137  }
138 
139  // Configure the solve.
140  ceres::Solver::Options solver_options;
141  solver_options.linear_solver_type = ceres::DENSE_QR;
142  solver_options.max_num_iterations = 50;
143  solver_options.update_state_every_iteration = true;
144  solver_options.parameter_tolerance = 1e-16;
145  solver_options.function_tolerance = 1e-16;
146 
147  // Run the solve.
148  ceres::Solver::Summary summary;
149  ceres::Solve(solver_options, &problem, &summary);
150 
151  VLOG(1) << "Summary:\n" << summary.FullReport();
152 
153  // Try projecting the point; make sure it's in front of everyone.
154  for (int i = 0; i < cameras.size(); ++i) {
155  const EuclideanCamera& camera =
157  Vec3 x = camera.R * X + camera.t;
158  if (x(2) < 0) {
159  LOG(ERROR) << "POINT BEHIND CAMERA " << markers[i].image << ": "
160  << x.transpose();
161  return false;
162  }
163  }
164 
165  Vec3 point = X.head<3>();
167 
168  // TODO(keir): Add proper error checking.
169  return true;
170 }
171 
172 namespace {
173 
174 struct ProjectiveIntersectCostFunction {
175  public:
176  typedef Vec FMatrixType;
177  typedef Vec4 XMatrixType;
178 
179  ProjectiveIntersectCostFunction(
180  const vector<Marker>& markers,
181  const ProjectiveReconstruction& reconstruction)
183 
184  Vec operator()(const Vec4& X) const {
185  Vec residuals(2 * markers.size());
186  residuals.setZero();
187  for (int i = 0; i < markers.size(); ++i) {
188  const ProjectiveCamera& camera =
189  *reconstruction.CameraForImage(markers[i].image);
190  Vec3 projected = camera.P * X;
191  projected /= projected(2);
192  residuals[2 * i + 0] = projected(0) - markers[i].x;
193  residuals[2 * i + 1] = projected(1) - markers[i].y;
194  }
195  return residuals;
196  }
198  const ProjectiveReconstruction& reconstruction;
199 };
200 
201 } // namespace
202 
205  if (markers.size() < 2) {
206  return false;
207  }
208 
209  // Get the cameras to use for the intersection.
210  vector<Mat34> cameras;
211  for (int i = 0; i < markers.size(); ++i) {
213  cameras.push_back(camera->P);
214  }
215 
216  // Stack the 2D coordinates together as required by NViewTriangulate.
217  Mat2X points(2, markers.size());
218  for (int i = 0; i < markers.size(); ++i) {
219  points(0, i) = markers[i].x;
220  points(1, i) = markers[i].y;
221  }
222 
223  Vec4 X;
224  LG << "Intersecting with " << markers.size() << " markers.";
225  NViewTriangulateAlgebraic(points, cameras, &X);
226  X /= X(3);
227 
229 
230  ProjectiveIntersectCostFunction triangulate_cost(markers, *reconstruction);
231  Solver::SolverParameters params;
232  Solver solver(triangulate_cost);
233 
234  Solver::Results results = solver.minimize(params, &X);
235  (void)results; // TODO(keir): Ensure results are good.
236 
237  // Try projecting the point; make sure it's in front of everyone.
238  for (int i = 0; i < cameras.size(); ++i) {
239  const ProjectiveCamera& camera =
241  Vec3 x = camera.P * X;
242  if (x(2) < 0) {
243  LOG(ERROR) << "POINT BEHIND CAMERA " << markers[i].image << ": "
244  << x.transpose();
245  }
246  }
247 
248  reconstruction->InsertPoint(markers[0].track, X);
249 
250  // TODO(keir): Add proper error checking.
251  return true;
252 }
253 
254 } // namespace libmv
#define K(key)
_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
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a point
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a or normal between camera
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
SIMD_FORCE_INLINE btVector3 operator()(const btVector3 &x) const
Return the transform of the vector.
Definition: btTransform.h:90
ProjectiveCamera * CameraForImage(int image)
Returns a pointer to the camera corresponding to image.
SyclQueue void void size_t num_bytes void
depth_tx normal_tx diffuse_light_tx specular_light_tx volume_light_tx environment_tx ambient_occlusion_tx aov_value_tx in_weight_img image(1, GPU_R32F, Qualifier::WRITE, ImageType::FLOAT_2D_ARRAY, "out_weight_img") .image(3
float[3][3] Mat3
Definition: gpu_matrix.cc:27
uiWidgetBaseParameters params[MAX_WIDGET_BASE_BATCH]
const ProjectiveReconstruction & reconstruction
Definition: intersect.cc:198
const vector< Marker > & markers
Definition: intersect.cc:197
const Marker & marker_
Definition: intersect.cc:63
const EuclideanCamera & camera_
Definition: intersect.cc:64
#define LG
#define VLOG(severity)
Definition: log.h:37
#define LOG(severity)
Definition: log.h:36
static float P(float k)
Definition: math_interp.c:25
#define T
#define R
Eigen::VectorXd Vec
Definition: numeric.h:61
void NViewTriangulateAlgebraic(const Matrix< T, 2, Dynamic > &x, const vector< Matrix< T, 3, 4 >> &Ps, Matrix< T, 4, 1 > *X)
Eigen::Vector4d Vec4
Definition: numeric.h:107
Eigen::Matrix< double, 3, 3 > Mat3
Definition: numeric.h:72
bool EuclideanIntersect(const vector< Marker > &markers, EuclideanReconstruction *reconstruction)
Definition: intersect.cc:69
Eigen::Matrix< double, 3, 4 > Mat34
Definition: numeric.h:73
Eigen::Vector3d Vec3
Definition: numeric.h:106
Eigen::Matrix< double, 2, Eigen::Dynamic > Mat2X
Definition: numeric.h:91
bool ProjectiveIntersect(const vector< Marker > &markers, ProjectiveReconstruction *reconstruction)
Definition: intersect.cc:203
void P_From_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t, Mat34 *P)
Definition: projection.cc:26