From e7fc35d66d906e17b9654babce218d6605ff4cbe Mon Sep 17 00:00:00 2001 From: Yann Noutary Date: Fri, 24 Jul 2020 11:31:44 +0200 Subject: [PATCH] fix: remove py::objects as return type and switch to STL types, as CLANG+pybind doesn't like py:: return types. --- opensfm/reconstruction.py | 10 +++--- opensfm/src/geometry/src/triangulation.cc | 39 ++++++++++------------- opensfm/src/geometry/triangulation.h | 31 +++++++----------- opensfm/test/test_triangulation.py | 4 +-- 4 files changed, 34 insertions(+), 50 deletions(-) diff --git a/opensfm/reconstruction.py b/opensfm/reconstruction.py index fbf19888c..cae66c550 100644 --- a/opensfm/reconstruction.py +++ b/opensfm/reconstruction.py @@ -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] @@ -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 @@ -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() @@ -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() @@ -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) diff --git a/opensfm/src/geometry/src/triangulation.cc b/opensfm/src/geometry/src/triangulation.cc index 38e2d9c64..c4eb4d50b 100644 --- a/opensfm/src/geometry/src/triangulation.cc +++ b/opensfm/src/geometry/src/triangulation.cc @@ -1,8 +1,6 @@ #include -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) @@ -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 &bearings, const std::vector > &Rts) { @@ -40,10 +31,12 @@ Eigen::Vector4d TriangulateBearingsDLTSolve( return worldPoint; } -py::object TriangulateBearingsDLT(const std::vector> &Rts, - const Eigen::Matrix &bearings, - double threshold, - double min_angle) { +namespace geometry { + +std::pair TriangulateBearingsDLT( + const std::vector> &Rts, + const Eigen::Matrix &bearings, double threshold, + double min_angle) { const int count = Rts.size(); Eigen::MatrixXd world_bearings(count, 3); bool angle_ok = false; @@ -59,7 +52,7 @@ py::object TriangulateBearingsDLT(const std::vector> } if (!angle_ok) { - return TriangulateReturn(TRIANGULATION_SMALL_ANGLE, py::none()); + return std::make_pair(false, Eigen::Vector3d()); } Eigen::Vector4d X = TriangulateBearingsDLTSolve(bearings, Rts); @@ -68,11 +61,11 @@ py::object TriangulateBearingsDLT(const std::vector> 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 TriangulateTwoBearingsMidpointMany( @@ -92,10 +85,10 @@ std::vector TriangulateTwoBearingsMidpointMany( return triangulated; } -py::object TriangulateBearingsMidpoint(const Eigen::Matrix ¢ers, - const Eigen::Matrix &bearings, - const std::vector&threshold_list, - double min_angle) { +std::pair TriangulateBearingsMidpoint( + const Eigen::Matrix ¢ers, + const Eigen::Matrix &bearings, + const std::vector &threshold_list, double min_angle) { const int count = centers.rows(); // Check angle between rays @@ -109,7 +102,7 @@ py::object TriangulateBearingsMidpoint(const Eigen::Matrix ¢e } } if (!angle_ok) { - return TriangulateReturn(TRIANGULATION_SMALL_ANGLE, py::none()); + return std::make_pair(false, Eigen::Vector3d()); } // Triangulate @@ -120,11 +113,11 @@ py::object TriangulateBearingsMidpoint(const Eigen::Matrix ¢e 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 diff --git a/opensfm/src/geometry/triangulation.h b/opensfm/src/geometry/triangulation.h index bfa0c0089..15cbc7a2f 100644 --- a/opensfm/src/geometry/triangulation.h +++ b/opensfm/src/geometry/triangulation.h @@ -9,28 +9,12 @@ #include #include -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 &bs, const std::vector< Eigen::Matrix > &Rts); -py::object TriangulateBearingsDLT(const std::vector> &Rts, - const Eigen::Matrix &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 @@ -59,6 +43,13 @@ Eigen::Matrix TriangulateBearingsMidpointSolve( return (Eigen::Matrix::Identity() + BBt * Cinv) * A / T(nviews) - Cinv * BBtA; } +namespace geometry { + +std::pair TriangulateBearingsDLT( + const std::vector> &Rts, + const Eigen::Matrix &bearings, double threshold, + double min_angle); + template< class T > Eigen::Matrix TriangulateTwoBearingsMidpointSolve( const Eigen::Matrix ¢ers, @@ -85,9 +76,9 @@ std::vector TriangulateTwoBearingsMidpointMany( const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation); -py::object TriangulateBearingsMidpoint(const Eigen::Matrix ¢ers, - const Eigen::Matrix &bearings, - const std::vector &threshold_list, - double min_angle); +std::pair TriangulateBearingsMidpoint( + const Eigen::Matrix ¢ers, + const Eigen::Matrix &bearings, + const std::vector &threshold_list, double min_angle); } // namespace geometry diff --git a/opensfm/test/test_triangulation.py b/opensfm/test/test_triangulation.py index 95da0b79c..f0d6c15d6 100644 --- a/opensfm/test/test_triangulation.py +++ b/opensfm/test/test_triangulation.py @@ -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(): @@ -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():