MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
triangulate.cc
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann
3 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
4 * All rights reserved.
5 *
6 * This software may be modified and distributed under the terms
7 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
8 */
9
10#include <stdexcept>
11
12#include "math/matrix_svd.h"
13#include "sfm/triangulate.h"
14
16
17/* ---------------- Low-level triangulation solver ---------------- */
18
21 CameraPose const& pose1, CameraPose const& pose2)
22{
23 /* The algorithm is described in HZ 12.2, page 312. */
25 pose1.fill_p_matrix(&P1);
26 pose2.fill_p_matrix(&P2);
27
29 for (int i = 0; i < 4; ++i)
30 {
31 A(0, i) = match.p1[0] * P1(2, i) - P1(0, i);
32 A(1, i) = match.p1[1] * P1(2, i) - P1(1, i);
33 A(2, i) = match.p2[0] * P2(2, i) - P2(0, i);
34 A(3, i) = match.p2[1] * P2(2, i) - P2(1, i);
35 }
36
38 math::matrix_svd<double, 4, 4>(A, nullptr, nullptr, &V);
40 return math::Vector<double, 3>(x[0] / x[3], x[1] / x[3], x[2] / x[3]);
41}
42
44triangulate_track (std::vector<math::Vec2f> const& pos,
45 std::vector<CameraPose const*> const& poses)
46{
47 if (pos.size() != poses.size() || pos.size() < 2)
48 throw std::invalid_argument("Invalid number of positions/poses");
49
50 std::vector<double> A(4 * 2 * poses.size(), 0.0);
51 for (std::size_t i = 0; i < poses.size(); ++i)
52 {
53 CameraPose const& pose = *poses[i];
54 math::Vec2d p = pos[i];
56 pose.fill_p_matrix(&p_mat);
57
58 for (int j = 0; j < 4; ++j)
59 {
60 A[(2 * i + 0) * 4 + j] = p[0] * p_mat(2, j) - p_mat(0, j);
61 A[(2 * i + 1) * 4 + j] = p[1] * p_mat(2, j) - p_mat(1, j);
62 }
63 }
64
65 /* Compute SVD. */
67 math::matrix_svd<double>(&A[0], 2 * poses.size(), 4,
68 nullptr, nullptr, mat_v.begin());
69
70 /* Consider the last column of V and extract 3D point. */
71 math::Vector<double, 4> x = mat_v.col(3);
72 return math::Vector<double, 3>(x[0] / x[3], x[1] / x[3], x[2] / x[3]);
73}
74
75bool
77 CameraPose const& pose1, CameraPose const& pose2)
78{
79 math::Vector<double, 3> x = triangulate_match(match, pose1, pose2);
80 math::Vector<double, 3> x1 = pose1.R * x + pose1.t;
81 math::Vector<double, 3> x2 = pose2.R * x + pose2.t;
82 return x1[2] > 0.0f && x2[2] > 0.0f;
83}
84
85/* --------------- Higher-level triangulation class --------------- */
86
87bool
88Triangulate::triangulate (std::vector<CameraPose const*> const& poses,
89 std::vector<math::Vec2f> const& positions,
90 math::Vec3d* track_pos, Statistics* stats,
91 std::vector<std::size_t>* outliers) const
92{
93 if (poses.size() < 2)
94 throw std::invalid_argument("At least two poses required");
95 if (poses.size() != positions.size())
96 throw std::invalid_argument("Poses and positions size mismatch");
97
98 /* Check all possible pose pairs for successful triangulation */
99 std::vector<std::size_t> best_outliers(positions.size());
100 math::Vec3f best_pos(0.0f);
101 for (std::size_t p1 = 0; p1 < poses.size(); ++p1)
102 for (std::size_t p2 = p1 + 1; p2 < poses.size(); ++p2)
103 {
104 /* Triangulate position from current pair */
105 std::vector<CameraPose const*> pose_pair;
106 std::vector<math::Vec2f> position_pair;
107 pose_pair.push_back(poses[p1]);
108 pose_pair.push_back(poses[p2]);
109 position_pair.push_back(positions[p1]);
110 position_pair.push_back(positions[p2]);
111 math::Vec3d tmp_pos = triangulate_track(position_pair, pose_pair);
112 if (MATH_ISNAN(tmp_pos[0]) || MATH_ISINF(tmp_pos[0]) ||
113 MATH_ISNAN(tmp_pos[1]) || MATH_ISINF(tmp_pos[1]) ||
114 MATH_ISNAN(tmp_pos[2]) || MATH_ISINF(tmp_pos[2]))
115 continue;
116
117 /* Check if pair has small triangulation angle. */
118 if (this->opts.angle_threshold > 0.0)
119 {
120 math::Vec3d camera_pos;
121 pose_pair[0]->fill_camera_pos(&camera_pos);
122 math::Vec3d ray0 = (tmp_pos - camera_pos).normalized();
123 pose_pair[1]->fill_camera_pos(&camera_pos);
124 math::Vec3d ray1 = (tmp_pos - camera_pos).normalized();
125 double const cos_angle = ray0.dot(ray1);
126 if (cos_angle > this->cos_angle_thres)
127 continue;
128 }
129
130 /* Check error in all input poses and find outliers. */
131 std::vector<std::size_t> tmp_outliers;
132 for (std::size_t i = 0; i < poses.size(); ++i)
133 {
134 math::Vec3d x = poses[i]->R * tmp_pos + poses[i]->t;
135
136 /* Reject track if it appears behind the camera. */
137 if (x[2] <= 0.0)
138 {
139 tmp_outliers.push_back(i);
140 continue;
141 }
142
143 x = poses[i]->K * x;
144 math::Vec2d x2d(x[0] / x[2], x[1] / x[2]);
145 double error = (positions[i] - x2d).norm();
146 if (error > this->opts.error_threshold)
147 tmp_outliers.push_back(i);
148 }
149
150 /* Select triangulation with lowest amount of outliers. */
151 if (tmp_outliers.size() < best_outliers.size())
152 {
153 best_pos = tmp_pos;
154 std::swap(best_outliers, tmp_outliers);
155 }
156
157 }
158
159 /* If all pairs have small angles pos will be 0 here. */
160 if (best_pos.norm() == 0.0f)
161 {
162 if (stats != nullptr)
163 stats->num_too_small_angle += 1;
164 return false;
165 }
166
167 /* Check if required number of inliers is found. */
168 if (poses.size() < best_outliers.size() + this->opts.min_num_views)
169 {
170 if (stats != nullptr)
171 stats->num_large_error += 1;
172 return false;
173 }
174
175 /* Return final position and outliers. */
176 *track_pos = best_pos;
177 if (stats != nullptr)
178 stats->num_new_tracks += 1;
179 if (outliers != nullptr)
180 std::swap(*outliers, best_outliers);
181
182 return true;
183}
184
185void
186Triangulate::print_statistics (Statistics const& stats, std::ostream& out) const
187{
188 int const num_rejected = stats.num_large_error
189 + stats.num_behind_camera
190 + stats.num_too_small_angle;
191
192 out << "Triangulated " << stats.num_new_tracks
193 << " new tracks, rejected " << num_rejected
194 << " bad tracks." << std::endl;
195 if (stats.num_large_error > 0)
196 out << " Rejected " << stats.num_large_error
197 << " tracks with large error." << std::endl;
198 if (stats.num_behind_camera > 0)
199 out << " Rejected " << stats.num_behind_camera
200 << " tracks behind cameras." << std::endl;
201 if (stats.num_too_small_angle > 0)
202 out << " Rejected " << stats.num_too_small_angle
203 << " tracks with unstable angle." << std::endl;
204}
205
Matrix class for arbitrary dimensions and types.
Definition matrix.h:54
Vector< T, N > col(int index) const
Returns a column of the matrix as vector.
Definition matrix.h:330
T * begin(void)
Definition matrix.h:506
Vector class for arbitrary dimensions and types.
Definition vector.h:87
T norm(void) const
Computes the norm (length) of the vector.
Definition vector.h:434
T dot(Vector< T, N > const &other) const
Dot (or scalar) product between self and another vector.
Definition vector.h:542
#define MATH_ISINF(x)
Definition defines.h:105
#define MATH_ISNAN(x)
Definition defines.h:104
math::Vector< double, 3 > triangulate_match(Correspondence2D2D const &match, CameraPose const &pose1, CameraPose const &pose2)
Given an image correspondence in two views and the corresponding poses, this function triangulates th...
math::Vector< double, 3 > triangulate_track(std::vector< math::Vec2f > const &pos, std::vector< CameraPose const * > const &poses)
Given any number of 2D image positions and the corresponding camera poses, this function triangulates...
bool is_consistent_pose(Correspondence2D2D const &match, CameraPose const &pose1, CameraPose const &pose2)
Given a two-view pose configuration and a correspondence, this function returns true if the triangula...
void swap(mve::Image< T > &a, mve::Image< T > &b)
Specialization of std::swap for efficient image swapping.
Definition image.h:478
#define SFM_NAMESPACE_END
Definition defines.h:14
#define SFM_NAMESPACE_BEGIN
Definition defines.h:13
The camera pose is the 3x4 matrix P = K [R | t].
Definition camera_pose.h:40
math::Matrix< double, 3, 3 > R
Definition camera_pose.h:59
void fill_p_matrix(math::Matrix< double, 3, 4 > *result) const
Returns the P matrix as the product K [R | t].
Definition camera_pose.h:81
math::Vector< double, 3 > t
Definition camera_pose.h:60
Two image coordinates which correspond to each other in terms of observing the same point in the scen...
int num_behind_camera
Number of tracks that appeared behind the camera.
Definition triangulate.h:82
int num_large_error
Number of tracks with too large reprojection error.
Definition triangulate.h:80
int num_too_small_angle
Number of tracks with too small triangulation angle.
Definition triangulate.h:84
int num_new_tracks
The number of successfully triangulated tracks.
Definition triangulate.h:78