29 for (
int i = 0; i < 4; ++i)
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);
38 math::matrix_svd<double, 4, 4>(A,
nullptr,
nullptr, &V);
45 std::vector<CameraPose const*>
const& poses)
47 if (pos.size() != poses.size() || pos.size() < 2)
48 throw std::invalid_argument(
"Invalid number of positions/poses");
50 std::vector<double> A(4 * 2 * poses.size(), 0.0);
51 for (std::size_t i = 0; i < poses.size(); ++i)
58 for (
int j = 0; j < 4; ++j)
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);
67 math::matrix_svd<double>(&A[0], 2 * poses.size(), 4,
68 nullptr,
nullptr, mat_v.
begin());
88Triangulate::triangulate (std::vector<CameraPose const*>
const& poses,
89 std::vector<math::Vec2f>
const& positions,
91 std::vector<std::size_t>* outliers)
const
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");
99 std::vector<std::size_t> best_outliers(positions.size());
101 for (std::size_t p1 = 0; p1 < poses.size(); ++p1)
102 for (std::size_t p2 = p1 + 1; p2 < poses.size(); ++p2)
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]);
118 if (this->opts.angle_threshold > 0.0)
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)
131 std::vector<std::size_t> tmp_outliers;
132 for (std::size_t i = 0; i < poses.size(); ++i)
134 math::Vec3d x = poses[i]->R * tmp_pos + poses[i]->t;
139 tmp_outliers.push_back(i);
145 double error = (positions[i] - x2d).norm();
146 if (error > this->opts.error_threshold)
147 tmp_outliers.push_back(i);
151 if (tmp_outliers.size() < best_outliers.size())
160 if (best_pos.
norm() == 0.0f)
162 if (stats !=
nullptr)
168 if (poses.size() < best_outliers.size() + this->opts.min_num_views)
170 if (stats !=
nullptr)
176 *track_pos = best_pos;
177 if (stats !=
nullptr)
179 if (outliers !=
nullptr)
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...