Skip to content

Commit

Permalink
Merge pull request mapillary#625 from mapillary/fix-pybind-clang
Browse files Browse the repository at this point in the history 
fix: pybing+CLANG
  • Loading branch information
@YanNoun
YanNoun authored Aug 7, 2020
2 parents 7cd27cb + e7fc35d commit 4456b5b
Show file tree
Hide file tree
Showing 4 changed files with 34 additions and 50 deletions.
10 changes: 5 additions & 5 deletions opensfm/reconstruction.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -553,7 +553,7 @@ def two_view_reconstruction(p1, p2, camera1, camera2,

if inliers.sum() > 5:
T = multiview.relative_pose_optimize_nonlinear(b1[inliers],
b2[inliers],
b2[inliers],
t, R,
iterations)
R = T[:, :3]
Expand Down Expand Up @@ -876,7 +876,7 @@ def triangulate_robust(self, track, reproj_threshold, min_ray_angle_degrees):
e, X = pygeometry.triangulate_bearings_midpoint(
os_t, bs_t, thresholds, np.radians(min_ray_angle_degrees))

if X is not None:
if e:
reprojected_bs = X-os
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < reproj_threshold
Expand Down Expand Up @@ -915,7 +915,7 @@ def triangulate(self, track, reproj_threshold, min_ray_angle_degrees):
thresholds = len(os) * [reproj_threshold]
e, X = pygeometry.triangulate_bearings_midpoint(
os, bs, thresholds, np.radians(min_ray_angle_degrees))
if X is not None:
if e:
point = types.Point()
point.id = track
point.coordinates = X.tolist()
Expand All @@ -937,7 +937,7 @@ def triangulate_dlt(self, track, reproj_threshold, min_ray_angle_degrees):
if len(Rts) >= 2:
e, X = pygeometry.triangulate_bearings_dlt(
Rts, bs, reproj_threshold, np.radians(min_ray_angle_degrees))
if X is not None:
if e:
point = types.Point()
point.id = track
point.coordinates = X.tolist()
Expand Down Expand Up @@ -1317,7 +1317,7 @@ def incremental_reconstruction(data, tracks_manager):
chrono = Chronometer()

images = tracks_manager.get_shot_ids()

if not data.reference_lla_exists():
data.invent_reference_lla(images)

Expand Down
39 changes: 16 additions & 23 deletions opensfm/src/geometry/src/triangulation.cc
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,6 @@

#include <geometry/triangulation.h>

namespace geometry {

double AngleBetweenVectors(const Eigen::Vector3d &u, const Eigen::Vector3d &v) {
double c = (u.dot(v)) / sqrt(u.dot(u) * v.dot(v));
if (std::fabs(c) >= 1.0)
Expand All @@ -11,13 +9,6 @@ double AngleBetweenVectors(const Eigen::Vector3d &u, const Eigen::Vector3d &v) {
return acos(c);
}

py::list TriangulateReturn(int error, py::object value) {
py::list retn;
retn.append(error);
retn.append(value);
return retn;
}

Eigen::Vector4d TriangulateBearingsDLTSolve(
const Eigen::Matrix<double, -1, 3> &bearings,
const std::vector<Eigen::Matrix<double, 3, 4> > &Rts) {
Expand All @@ -40,10 +31,12 @@ Eigen::Vector4d TriangulateBearingsDLTSolve(
return worldPoint;
}

py::object TriangulateBearingsDLT(const std::vector<Eigen::Matrix<double, 3, 4>> &Rts,
const Eigen::Matrix<double, -1, 3> &bearings,
double threshold,
double min_angle) {
namespace geometry {

std::pair<bool, Eigen::Vector3d> TriangulateBearingsDLT(
const std::vector<Eigen::Matrix<double, 3, 4>> &Rts,
const Eigen::Matrix<double, -1, 3> &bearings, double threshold,
double min_angle) {
const int count = Rts.size();
Eigen::MatrixXd world_bearings(count, 3);
bool angle_ok = false;
Expand All @@ -59,7 +52,7 @@ py::object TriangulateBearingsDLT(const std::vector<Eigen::Matrix<double, 3, 4>>
}

if (!angle_ok) {
return TriangulateReturn(TRIANGULATION_SMALL_ANGLE, py::none());
return std::make_pair(false, Eigen::Vector3d());
}

Eigen::Vector4d X = TriangulateBearingsDLTSolve(bearings, Rts);
Expand All @@ -68,11 +61,11 @@ py::object TriangulateBearingsDLT(const std::vector<Eigen::Matrix<double, 3, 4>>
for (int i = 0; i < count; ++i) {
const Eigen::Vector3d projected = Rts[i] * X;
if (AngleBetweenVectors(projected, bearings.row(i)) > threshold) {
return TriangulateReturn(TRIANGULATION_BAD_REPROJECTION, py::none());
return std::make_pair(false, Eigen::Vector3d());
}
}

return TriangulateReturn(TRIANGULATION_OK, foundation::py_array_from_data(X.data(), 3));
return std::make_pair(true, X.head<3>());
}

std::vector<Eigen::Vector3d> TriangulateTwoBearingsMidpointMany(
Expand All @@ -92,10 +85,10 @@ std::vector<Eigen::Vector3d> TriangulateTwoBearingsMidpointMany(
return triangulated;
}

py::object TriangulateBearingsMidpoint(const Eigen::Matrix<double, -1, 3> &centers,
const Eigen::Matrix<double, -1, 3> &bearings,
const std::vector<double>&threshold_list,
double min_angle) {
std::pair<bool, Eigen::Vector3d> TriangulateBearingsMidpoint(
const Eigen::Matrix<double, -1, 3> &centers,
const Eigen::Matrix<double, -1, 3> &bearings,
const std::vector<double> &threshold_list, double min_angle) {
const int count = centers.rows();

// Check angle between rays
Expand All @@ -109,7 +102,7 @@ py::object TriangulateBearingsMidpoint(const Eigen::Matrix<double, -1, 3> &cente
}
}
if (!angle_ok) {
return TriangulateReturn(TRIANGULATION_SMALL_ANGLE, py::none());
return std::make_pair(false, Eigen::Vector3d());
}

// Triangulate
Expand All @@ -120,11 +113,11 @@ py::object TriangulateBearingsMidpoint(const Eigen::Matrix<double, -1, 3> &cente
const Eigen::Vector3d projected = X - centers.row(i).transpose();
const Eigen::Vector3d measured = bearings.row(i);
if (AngleBetweenVectors(projected, measured) > threshold_list[i]) {
return TriangulateReturn(TRIANGULATION_BAD_REPROJECTION, py::none());
return std::make_pair(false, Eigen::Vector3d());
}
}

return TriangulateReturn(TRIANGULATION_OK, foundation::py_array_from_data(X.data(), 3));
return std::make_pair(true, X.head<3>());
}

} // namespace geometry
31 changes: 11 additions & 20 deletions opensfm/src/geometry/triangulation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,28 +9,12 @@
#include <string>
#include <foundation/python_types.h>

namespace geometry {

enum {
TRIANGULATION_OK = 0,
TRIANGULATION_SMALL_ANGLE,
TRIANGULATION_BEHIND_CAMERA,
TRIANGULATION_BAD_REPROJECTION
};

double AngleBetweenVectors(const Eigen::Vector3d &u, const Eigen::Vector3d &v);

py::list TriangulateReturn(int error, py::object value);

Eigen::Vector4d TriangulateBearingsDLTSolve(
const Eigen::Matrix<double, -1, 3> &bs,
const std::vector< Eigen::Matrix<double, 3, 4> > &Rts);

py::object TriangulateBearingsDLT(const std::vector<Eigen::Matrix<double, 3, 4>> &Rts,
const Eigen::Matrix<double, -1, 3> &bearings,
double threshold,
double min_angle);

// Point minimizing the squared distance to all rays
// Closed for solution from
// Srikumar Ramalingam, Suresh K. Lodha and Peter Sturm
Expand Down Expand Up @@ -59,6 +43,13 @@ Eigen::Matrix<T, 3, 1> TriangulateBearingsMidpointSolve(
return (Eigen::Matrix<T, 3, 3>::Identity() + BBt * Cinv) * A / T(nviews) - Cinv * BBtA;
}

namespace geometry {

std::pair<bool, Eigen::Vector3d> TriangulateBearingsDLT(
const std::vector<Eigen::Matrix<double, 3, 4>> &Rts,
const Eigen::Matrix<double, -1, 3> &bearings, double threshold,
double min_angle);

template< class T >
Eigen::Matrix<T, 3, 1> TriangulateTwoBearingsMidpointSolve(
const Eigen::Matrix<T, 2, 3> &centers,
Expand All @@ -85,9 +76,9 @@ std::vector<Eigen::Vector3d> TriangulateTwoBearingsMidpointMany(
const Eigen::Matrix3d& rotation,
const Eigen::Vector3d& translation);

py::object TriangulateBearingsMidpoint(const Eigen::Matrix<double, -1, 3> &centers,
const Eigen::Matrix<double, -1, 3> &bearings,
const std::vector<double> &threshold_list,
double min_angle);
std::pair<bool, Eigen::Vector3d> TriangulateBearingsMidpoint(
const Eigen::Matrix<double, -1, 3> &centers,
const Eigen::Matrix<double, -1, 3> &bearings,
const std::vector<double> &threshold_list, double min_angle);

} // namespace geometry
4 changes: 2 additions & 2 deletions opensfm/test/test_triangulation.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ def test_triangulate_bearings_dlt():
res, X = pygeometry.triangulate_bearings_dlt(
[rt1, rt2], [b1, b2], max_reprojection, min_ray_angle)
assert np.allclose(X, [0, 0, 1.0])
assert res == 0
assert res is True


def test_triangulate_bearings_midpoint():
Expand All @@ -75,7 +75,7 @@ def test_triangulate_bearings_midpoint():
res, X = pygeometry.triangulate_bearings_midpoint(
[o1, o2], [b1, b2], 2 * [max_reprojection], min_ray_angle)
assert np.allclose(X, [0, 0, 1.0])
assert res == 0
assert res is True


def test_triangulate_two_bearings_midpoint():
Expand Down

0 comments on commit 4456b5b

Please sign in to comment.