diff --git a/limap/_limap/bindings.cc b/limap/_limap/bindings.cc index 0b054b29..daa7425e 100644 --- a/limap/_limap/bindings.cc +++ b/limap/_limap/bindings.cc @@ -45,7 +45,7 @@ void bind_features(py::module &); namespace limap { PYBIND11_MODULE(_limap, m) { - m.doc() = "Multi-view Line Triangulation and Refinement"; + m.doc() = "A toolbox for mapping and localization with line features"; #ifdef VERSION_INFO m.attr("__version__") = py::str(VERSION_INFO); #else diff --git a/limap/estimators/absolute_pose/hybrid_pose_estimator.cc b/limap/estimators/absolute_pose/hybrid_pose_estimator.cc index e9beb631..bd2010af 100644 --- a/limap/estimators/absolute_pose/hybrid_pose_estimator.cc +++ b/limap/estimators/absolute_pose/hybrid_pose_estimator.cc @@ -1,7 +1,7 @@ #include "estimators/absolute_pose/hybrid_pose_estimator.h" #include "estimators/absolute_pose/pl_absolute_pose_hybrid_ransac.h" #include "estimators/absolute_pose/pl_absolute_pose_ransac.h" -#include "optimize/line_localization/cost_functions.h" +#include "optimize/hybrid_localization/cost_functions.h" namespace limap { @@ -9,7 +9,7 @@ namespace estimators { namespace absolute_pose { -namespace lineloc = optimize::line_localization; +namespace hybridloc = optimize::hybrid_localization; std::pair EstimateAbsolutePose_PointLine_Hybrid( diff --git a/limap/estimators/absolute_pose/hybrid_pose_estimator.h b/limap/estimators/absolute_pose/hybrid_pose_estimator.h index f92aae6e..45f1b7ee 100644 --- a/limap/estimators/absolute_pose/hybrid_pose_estimator.h +++ b/limap/estimators/absolute_pose/hybrid_pose_estimator.h @@ -6,14 +6,14 @@ #include "base/linetrack.h" #include "estimators/absolute_pose/joint_pose_estimator.h" #include "estimators/extended_hybrid_ransac.h" -#include "optimize/line_localization/lineloc.h" +#include "optimize/hybrid_localization/hybrid_localization.h" #include "util/types.h" #include namespace limap { -using namespace optimize::line_localization; +using namespace optimize::hybrid_localization; namespace estimators { diff --git a/limap/estimators/absolute_pose/joint_pose_estimator.cc b/limap/estimators/absolute_pose/joint_pose_estimator.cc index 87860372..16d4ab2b 100644 --- a/limap/estimators/absolute_pose/joint_pose_estimator.cc +++ b/limap/estimators/absolute_pose/joint_pose_estimator.cc @@ -4,7 +4,7 @@ #include "ceresbase/line_projection.h" #include "ceresbase/point_projection.h" #include "estimators/absolute_pose/pl_absolute_pose_ransac.h" -#include "optimize/line_localization/cost_functions.h" +#include "optimize/hybrid_localization/cost_functions.h" #include #include @@ -18,7 +18,7 @@ namespace estimators { namespace absolute_pose { -namespace lineloc = optimize::line_localization; +namespace hybridloc = optimize::hybrid_localization; std::pair EstimateAbsolutePose_PointLine(const std::vector &l3ds, @@ -60,7 +60,7 @@ JointPoseEstimator::JointPoseEstimator( const std::vector &l3ds, const std::vector &l3d_ids, const std::vector &l2ds, const std::vector &p3ds, const std::vector &p2ds, const Camera &cam, - const lineloc::LineLocConfig &cfg, const double cheirality_min_depth, + const hybridloc::LineLocConfig &cfg, const double cheirality_min_depth, const double cheirality_overlap_pixels) : loc_config_(cfg), cheirality_min_depth_(cheirality_min_depth), cheirality_overlap_pixels_(cheirality_overlap_pixels) { @@ -182,8 +182,8 @@ double JointPoseEstimator::EvaluateModelOnPoint(const CameraPose &pose, bool cheirality = cheirality_test_point(p3ds_->at(i), pose); if (!cheirality) return std::numeric_limits::max(); - lineloc::ReprojectionPointFunctor(p3ds_->at(i), p2ds_->at(i), - loc_config_.points_3d_dist)( + hybridloc::ReprojectionPointFunctor(p3ds_->at(i), p2ds_->at(i), + loc_config_.points_3d_dist)( cam_.Params().data(), pose.qvec.data(), pose.tvec.data(), res); return V2D(res[0], res[1]).squaredNorm(); } else { @@ -193,8 +193,8 @@ double JointPoseEstimator::EvaluateModelOnPoint(const CameraPose &pose, bool cheirality = cheirality_test_line(l2ds_->at(i), l3d, pose); if (!cheirality) return std::numeric_limits::max(); - lineloc::ReprojectionLineFunctor(loc_config_.cost_function, ENoneWeight, - l3d, l2ds_->at(i))( + hybridloc::ReprojectionLineFunctor(loc_config_.cost_function, ENoneWeight, + l3d, l2ds_->at(i))( cam_.Params().data(), pose.qvec.data(), pose.tvec.data(), res); if (getResidualNum(loc_config_.cost_function) == 2) { return V2D(res[0], res[1]).squaredNorm(); diff --git a/limap/estimators/absolute_pose/joint_pose_estimator.h b/limap/estimators/absolute_pose/joint_pose_estimator.h index 2b47f2c9..2aa41987 100644 --- a/limap/estimators/absolute_pose/joint_pose_estimator.h +++ b/limap/estimators/absolute_pose/joint_pose_estimator.h @@ -4,14 +4,14 @@ #include "_limap/helpers.h" #include "base/camera.h" #include "base/linetrack.h" -#include "optimize/line_localization/lineloc.h" +#include "optimize/hybrid_localization/hybrid_localization.h" #include "util/types.h" #include namespace limap { -using namespace optimize::line_localization; +using namespace optimize::hybrid_localization; namespace estimators { diff --git a/limap/optimize/CMakeLists.txt b/limap/optimize/CMakeLists.txt index b75e44fb..e7d4137a 100644 --- a/limap/optimize/CMakeLists.txt +++ b/limap/optimize/CMakeLists.txt @@ -3,7 +3,7 @@ set(FOLDER_NAME "optimize") add_subdirectory(line_refinement) add_subdirectory(hybrid_bundle_adjustment) add_subdirectory(global_pl_association) -add_subdirectory(line_localization) +add_subdirectory(hybrid_localization) LIMAP_ADD_SOURCES( bindings.cc diff --git a/limap/optimize/__init__.py b/limap/optimize/__init__.py index 98c6f19e..d3a1c7fc 100644 --- a/limap/optimize/__init__.py +++ b/limap/optimize/__init__.py @@ -7,7 +7,7 @@ solve_line_bundle_adjustment, solve_point_bundle_adjustment, ) -from .line_localization import ( +from .hybrid_localization import ( get_lineloc_cost_func, get_lineloc_weight_func, solve_jointloc, diff --git a/limap/optimize/bindings.cc b/limap/optimize/bindings.cc index 2b23bc6c..61caed00 100644 --- a/limap/optimize/bindings.cc +++ b/limap/optimize/bindings.cc @@ -9,7 +9,7 @@ #include "optimize/global_pl_association/bindings.cc" #include "optimize/hybrid_bundle_adjustment/bindings.cc" -#include "optimize/line_localization/bindings.cc" +#include "optimize/hybrid_localization/bindings.cc" #include "optimize/line_refinement/bindings.cc" namespace py = pybind11; @@ -19,13 +19,13 @@ namespace limap { void bind_line_refinement(py::module &m); void bind_hybrid_bundle_adjustment(py::module &m); void bind_global_pl_association(py::module &m); -void bind_line_localization(py::module &m); +void bind_hybrid_localization(py::module &m); void bind_optimize(py::module &m) { bind_line_refinement(m); bind_hybrid_bundle_adjustment(m); bind_global_pl_association(m); - bind_line_localization(m); + bind_hybrid_localization(m); } } // namespace limap diff --git a/limap/optimize/hybrid_localization/CMakeLists.txt b/limap/optimize/hybrid_localization/CMakeLists.txt new file mode 100644 index 00000000..95038e81 --- /dev/null +++ b/limap/optimize/hybrid_localization/CMakeLists.txt @@ -0,0 +1,9 @@ +set(FOLDER_NAME "hybrid_localization") + +LIMAP_ADD_SOURCES( + bindings.cc + cost_functions.h + hybrid_localization.h hybrid_localization.cc + hybrid_localization_config.h +) + diff --git a/limap/optimize/line_localization/__init__.py b/limap/optimize/hybrid_localization/__init__.py similarity index 100% rename from limap/optimize/line_localization/__init__.py rename to limap/optimize/hybrid_localization/__init__.py diff --git a/limap/optimize/line_localization/bindings.cc b/limap/optimize/hybrid_localization/bindings.cc similarity index 93% rename from limap/optimize/line_localization/bindings.cc rename to limap/optimize/hybrid_localization/bindings.cc index aa0fe912..a617bd33 100644 --- a/limap/optimize/line_localization/bindings.cc +++ b/limap/optimize/hybrid_localization/bindings.cc @@ -7,15 +7,15 @@ #include #include -#include "optimize/line_localization/lineloc.h" -#include "optimize/line_localization/lineloc_config.h" +#include "optimize/hybrid_localization/hybrid_localization.h" +#include "optimize/hybrid_localization/hybrid_localization_config.h" namespace py = pybind11; namespace limap { void bind_lineloc_engine(py::module &m) { - using namespace optimize::line_localization; + using namespace optimize::hybrid_localization; using LocEngine = LineLocEngine; using JointEngine = JointLocEngine; @@ -63,8 +63,8 @@ void bind_lineloc_engine(py::module &m) { .def("GetFinalCost", &JointEngine::GetFinalCost); } -void bind_line_localization(py::module &m) { - using namespace optimize::line_localization; +void bind_hybrid_localization(py::module &m) { + using namespace optimize::hybrid_localization; py::enum_(m, "LineLocCostFunction") .value("E2DMidpointDist2", LineLocCostFunction::E2DMidpointDist2) diff --git a/limap/optimize/line_localization/cost_functions.h b/limap/optimize/hybrid_localization/cost_functions.h similarity index 97% rename from limap/optimize/line_localization/cost_functions.h rename to limap/optimize/hybrid_localization/cost_functions.h index e91d8ba4..930aad48 100644 --- a/limap/optimize/line_localization/cost_functions.h +++ b/limap/optimize/hybrid_localization/cost_functions.h @@ -1,5 +1,5 @@ -#ifndef LIMAP_OPTIMIZE_LINE_LOCALIZATION_COST_FUNCTIONS_H_ -#define LIMAP_OPTIMIZE_LINE_LOCALIZATION_COST_FUNCTIONS_H_ +#ifndef LIMAP_OPTIMIZE_HYBRID_LOCALIZATION_COST_FUNCTIONS_H_ +#define LIMAP_OPTIMIZE_HYBRID_LOCALIZATION_COST_FUNCTIONS_H_ #include "_limap/helpers.h" #include @@ -14,7 +14,7 @@ #include "ceresbase/point_projection.h" #include -#include "optimize/line_localization/lineloc_config.h" +#include "optimize/hybrid_localization/hybrid_localization_config.h" namespace py = pybind11; @@ -22,7 +22,7 @@ namespace limap { namespace optimize { -namespace line_localization { +namespace hybrid_localization { //////////////////////////////////////////////////////////// // 2D Weights and Line Dists @@ -347,7 +347,7 @@ struct ReprojectionPointFunctor { bool use_3d_dist_; }; -} // namespace line_localization +} // namespace hybrid_localization } // namespace optimize diff --git a/limap/optimize/line_localization/functions.py b/limap/optimize/hybrid_localization/functions.py similarity index 100% rename from limap/optimize/line_localization/functions.py rename to limap/optimize/hybrid_localization/functions.py diff --git a/limap/optimize/line_localization/lineloc.cc b/limap/optimize/hybrid_localization/hybrid_localization.cc similarity index 96% rename from limap/optimize/line_localization/lineloc.cc rename to limap/optimize/hybrid_localization/hybrid_localization.cc index d56cf256..ac209018 100644 --- a/limap/optimize/line_localization/lineloc.cc +++ b/limap/optimize/hybrid_localization/hybrid_localization.cc @@ -1,6 +1,6 @@ -#include "optimize/line_localization/lineloc.h" +#include "optimize/hybrid_localization/hybrid_localization.h" #include "ceresbase/parameterization.h" -#include "optimize/line_localization/cost_functions.h" +#include "optimize/hybrid_localization/cost_functions.h" #include #include @@ -11,7 +11,7 @@ namespace limap { namespace optimize { -namespace line_localization { +namespace hybrid_localization { void LineLocEngine::ParameterizeCamera() { double *kvec_data = cam.Params().data(); @@ -134,7 +134,7 @@ void JointLocEngine::AddResiduals() { } } -} // namespace line_localization +} // namespace hybrid_localization } // namespace optimize diff --git a/limap/optimize/line_localization/lineloc.h b/limap/optimize/hybrid_localization/hybrid_localization.h similarity index 93% rename from limap/optimize/line_localization/lineloc.h rename to limap/optimize/hybrid_localization/hybrid_localization.h index 37d5a00e..ff5fcfec 100644 --- a/limap/optimize/line_localization/lineloc.h +++ b/limap/optimize/hybrid_localization/hybrid_localization.h @@ -1,5 +1,5 @@ -#ifndef LIMAP_OPTIMIZE_LINE_LOCALIZATION_LINELOC_H_ -#define LIMAP_OPTIMIZE_LINE_LOCALIZATION_LINELOC_H_ +#ifndef LIMAP_OPTIMIZE_HYBRID_LOCALIZATION_HYBRID_LOCALIZATION_H_ +#define LIMAP_OPTIMIZE_HYBRID_LOCALIZATION_HYBRID_LOCALIZATION_H_ #include "_limap/helpers.h" #include @@ -8,7 +8,7 @@ #include "util/types.h" #include "base/linebase.h" -#include "optimize/line_localization/lineloc_config.h" +#include "optimize/hybrid_localization/hybrid_localization_config.h" #include namespace py = pybind11; @@ -17,7 +17,7 @@ namespace limap { namespace optimize { -namespace line_localization { +namespace hybrid_localization { class LineLocEngine { protected: @@ -126,7 +126,7 @@ class JointLocEngine : public LineLocEngine { } }; -} // namespace line_localization +} // namespace hybrid_localization } // namespace optimize diff --git a/limap/optimize/line_localization/lineloc_config.h b/limap/optimize/hybrid_localization/hybrid_localization_config.h similarity index 91% rename from limap/optimize/line_localization/lineloc_config.h rename to limap/optimize/hybrid_localization/hybrid_localization_config.h index b1d6b08d..3118bdf3 100644 --- a/limap/optimize/line_localization/lineloc_config.h +++ b/limap/optimize/hybrid_localization/hybrid_localization_config.h @@ -1,5 +1,5 @@ -#ifndef LIMAP_OPTIMIZE_LINE_LOCALIZATION_LINELOC_CONFIG_H_ -#define LIMAP_OPTIMIZE_LINE_LOCALIZATION_LINELOC_CONFIG_H_ +#ifndef LIMAP_OPTIMIZE_HYBRID_LOCALIZATION_HYBRID_LOCALIZATION_CONFIG_H_ +#define LIMAP_OPTIMIZE_HYBRID_LOCALIZATION_HYBRID_LOCALIZATION_CONFIG_H_ #include "_limap/helpers.h" #include @@ -14,7 +14,7 @@ namespace limap { namespace optimize { -namespace line_localization { +namespace hybrid_localization { enum LineLocCostFunction { E2DMidpointDist2, @@ -78,7 +78,7 @@ class LineLocConfig { std::shared_ptr loss_function; }; -} // namespace line_localization +} // namespace hybrid_localization } // namespace optimize diff --git a/limap/optimize/line_localization/solve.py b/limap/optimize/hybrid_localization/solve.py similarity index 100% rename from limap/optimize/line_localization/solve.py rename to limap/optimize/hybrid_localization/solve.py diff --git a/limap/optimize/line_localization/CMakeLists.txt b/limap/optimize/line_localization/CMakeLists.txt deleted file mode 100644 index ead199f1..00000000 --- a/limap/optimize/line_localization/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -set(FOLDER_NAME "line_localization") - -LIMAP_ADD_SOURCES( - bindings.cc - cost_functions.h - lineloc.h lineloc.cc - lineloc_config.h -) - diff --git a/limap/runners/__init__.py b/limap/runners/__init__.py index 9932a6e4..7728e3b7 100644 --- a/limap/runners/__init__.py +++ b/limap/runners/__init__.py @@ -7,17 +7,17 @@ undistort_images, ) from .functions_structures import compute_2d_bipartites_from_colmap +from .hybrid_localization import ( + get_hloc_keypoints, + get_hloc_keypoints_from_log, + hybrid_localization, +) from .line_fitnmerge import ( fit_3d_segs, fit_3d_segs_with_points3d, line_fitnmerge, line_fitting_with_points3d, ) -from .line_localization import ( - get_hloc_keypoints, - get_hloc_keypoints_from_log, - line_localization, -) from .line_triangulation import line_triangulation __all__ = [ @@ -34,6 +34,6 @@ "line_fitting_with_points3d", "get_hloc_keypoints", "get_hloc_keypoints_from_log", - "line_localization", + "hybrid_localization", "line_triangulation", ] diff --git a/limap/runners/line_localization.py b/limap/runners/hybrid_localization.py similarity index 99% rename from limap/runners/line_localization.py rename to limap/runners/hybrid_localization.py index b83d810f..02c0baec 100644 --- a/limap/runners/line_localization.py +++ b/limap/runners/hybrid_localization.py @@ -11,7 +11,7 @@ import limap.line2d import limap.runners as runners import limap.util.io as limapio -from limap.optimize.line_localization.functions import ( +from limap.optimize.hybrid_localization.functions import ( filter_line_2to2_epipolarIoU, get_reprojection_dist_func, match_line_2to2_epipolarIoU, @@ -91,7 +91,7 @@ def get_hloc_keypoints_from_log( return p2ds, p3ds, inliers -def line_localization( +def hybrid_localization( cfg, imagecols_db, imagecols_query, diff --git a/limap/triangulation/__init__.py b/limap/triangulation/__init__.py index 4f83dfc2..8295dce8 100644 --- a/limap/triangulation/__init__.py +++ b/limap/triangulation/__init__.py @@ -7,11 +7,11 @@ compute_fundamental_matrix, get_direction_from_VP, get_normal_direction, - point_triangulation, - triangulate, - triangulate_endpoints, - triangulate_with_direction, - triangulate_with_one_point, + triangulate_line, + triangulate_line_by_endpoints, + triangulate_line_with_direction, + triangulate_line_with_one_point, + triangulate_point, ) __all__ = [n for n in _triangulation.__dict__ if not n.startswith("_")] + [ @@ -20,9 +20,9 @@ "compute_essential_matrix", "compute_fundamental_matrix", "compute_epipolar_IoU", - "point_triangulation", - "triangulate_endpoints", - "triangulate", - "triangulate_with_one_point", - "triangulate_with_direction", + "triangulate_point", + "triangulate_line_by_endpoints", + "triangulate_line", + "triangulate_line_with_one_point", + "triangulate_line_with_direction", ] diff --git a/limap/triangulation/base_line_triangulator.cc b/limap/triangulation/base_line_triangulator.cc index 5d7b450a..30996cc3 100644 --- a/limap/triangulation/base_line_triangulator.cc +++ b/limap/triangulation/base_line_triangulator.cc @@ -183,8 +183,8 @@ void BaseLineTriangulator::triangulateOneNode(const int img_id, std::vector points; for (auto it = points_info.begin(); it != points_info.end(); ++it) { if (sfm_points_.empty()) { - auto res = point_triangulation(it->second.first, view1, - it->second.second, view2); + auto res = triangulate_point(it->second.first, view1, + it->second.second, view2); if (res.second) points.push_back(res.first); } else @@ -208,7 +208,7 @@ void BaseLineTriangulator::triangulateOneNode(const int img_id, Eigen::JacobiSVD svd(epoints, Eigen::ComputeThinV); V3D direc = svd.matrixV().col(0).normalized(); InfiniteLine3d inf_line = InfiniteLine3d(center, direc); - Line3d line = triangulate_with_infinite_line(l1, view1, inf_line); + Line3d line = triangulate_line_with_infinite_line(l1, view1, inf_line); if (line.score > 0) { double u1 = line.computeUncertainty(view1, config_.var2d); double u2 = line.computeUncertainty(view2, config_.var2d); @@ -221,7 +221,8 @@ void BaseLineTriangulator::triangulateOneNode(const int img_id, // Step 1.2 one point triangulation if (!config_.disable_one_point_triangulation && !points.empty()) { for (const V3D &p : points) { - Line3d line = triangulate_with_one_point(l1, view1, l2, view2, p); + Line3d line = + triangulate_line_with_one_point(l1, view1, l2, view2, p); if (line.score > 0) { double u1 = line.computeUncertainty(view1, config_.var2d); double u2 = line.computeUncertainty(view2, config_.var2d); @@ -239,7 +240,8 @@ void BaseLineTriangulator::triangulateOneNode(const int img_id, if (vpresults_[img_id].HasVP(line_id)) { V3D direc = getDirectionFromVP(vpresults_[img_id].GetVP(line_id), view1); - Line3d line = triangulate_with_direction(l1, view1, l2, view2, direc); + Line3d line = + triangulate_line_with_direction(l1, view1, l2, view2, direc); if (line.score > 0) { double u1 = line.computeUncertainty(view1, config_.var2d); double u2 = line.computeUncertainty(view2, config_.var2d); @@ -252,7 +254,8 @@ void BaseLineTriangulator::triangulateOneNode(const int img_id, if (vpresults_[ng_img_id].HasVP(ng_line_id)) { V3D direc = getDirectionFromVP(vpresults_[ng_img_id].GetVP(ng_line_id), view1); - Line3d line = triangulate_with_direction(l1, view1, l2, view2, direc); + Line3d line = + triangulate_line_with_direction(l1, view1, l2, view2, direc); if (line.score > 0) { double u1 = line.computeUncertainty(view1, config_.var2d); double u2 = line.computeUncertainty(view2, config_.var2d); @@ -285,9 +288,9 @@ void BaseLineTriangulator::triangulateOneNode(const int img_id, // triangulation with weak epipolar constraints test Line3d line; if (!config_.use_endpoints_triangulation) - line = triangulate(l1, view1, l2, view2); + line = triangulate_line(l1, view1, l2, view2); else - line = triangulate_endpoints(l1, view1, l2, view2); + line = triangulate_line_by_endpoints(l1, view1, l2, view2); if (line.sensitivity(view1) > config_.sensitivity_threshold && line.sensitivity(view2) > config_.sensitivity_threshold) line.score = -1; diff --git a/limap/triangulation/bindings.cc b/limap/triangulation/bindings.cc index c8fd90ff..a41b976c 100644 --- a/limap/triangulation/bindings.cc +++ b/limap/triangulation/bindings.cc @@ -24,11 +24,11 @@ void bind_functions(py::module &m) { m.def("compute_essential_matrix", &compute_essential_matrix); m.def("compute_fundamental_matrix", &compute_fundamental_matrix); m.def("compute_epipolar_IoU", &compute_epipolar_IoU); - m.def("point_triangulation", &point_triangulation); - m.def("triangulate_endpoints", &triangulate_endpoints); - m.def("triangulate", &triangulate); - m.def("triangulate_with_one_point", &triangulate_with_one_point); - m.def("triangulate_with_direction", &triangulate_with_direction); + m.def("triangulate_point", &triangulate_point); + m.def("triangulate_line_by_endpoints", &triangulate_line_by_endpoints); + m.def("triangulate_line", &triangulate_line); + m.def("triangulate_line_with_one_point", &triangulate_line_with_one_point); + m.def("triangulate_line_with_direction", &triangulate_line_with_direction); } void bind_triangulator(py::module &m) { diff --git a/limap/triangulation/functions.cc b/limap/triangulation/functions.cc index 9764eaae..a48ad815 100644 --- a/limap/triangulation/functions.cc +++ b/limap/triangulation/functions.cc @@ -97,9 +97,8 @@ double compute_epipolar_IoU(const Line2d &l1, const CameraView &view1, return IoU; } -std::pair point_triangulation(const V2D &p1, const CameraView &view1, - const V2D &p2, - const CameraView &view2) { +std::pair triangulate_point(const V2D &p1, const CameraView &view1, + const V2D &p2, const CameraView &view2) { V3D C1 = view1.pose.center(); V3D C2 = view2.pose.center(); V3D n1e = view1.ray_direction(p1); @@ -170,15 +169,16 @@ M3D point_triangulation_covariance(const V2D &p1, const CameraView &view1, } // Triangulating endpoints for triangulation -Line3d triangulate_endpoints(const Line2d &l1, const CameraView &view1, - const Line2d &l2, const CameraView &view2) { +Line3d triangulate_line_by_endpoints(const Line2d &l1, const CameraView &view1, + const Line2d &l2, + const CameraView &view2) { // start point - auto res_start = point_triangulation(l1.start, view1, l2.start, view2); + auto res_start = triangulate_point(l1.start, view1, l2.start, view2); if (!res_start.second) return Line3d(V3D(0, 0, 0), V3D(1, 1, 1), -1.0); V3D pstart = res_start.first; // end point - auto res_end = point_triangulation(l1.end, view1, l2.end, view2); + auto res_end = triangulate_point(l1.end, view1, l2.end, view2); if (!res_end.second) return Line3d(V3D(0, 0, 0), V3D(1, 1, 1), -1.0); V3D pend = res_end.first; @@ -292,8 +292,8 @@ M6D line_triangulation_covariance(const Line2d &l1, const CameraView &view1, } // Algebraic line triangulation -Line3d triangulate(const Line2d &l1, const CameraView &view1, const Line2d &l2, - const CameraView &view2) { +Line3d triangulate_line(const Line2d &l1, const CameraView &view1, + const Line2d &l2, const CameraView &view2) { // triangulate line auto res = line_triangulation(l1, view1, l2, view2); if (!res.second) @@ -303,8 +303,9 @@ Line3d triangulate(const Line2d &l1, const CameraView &view1, const Line2d &l2, } // unproject endpoints with known infinite line -Line3d triangulate_with_infinite_line(const Line2d &l1, const CameraView &view1, - const InfiniteLine3d &inf_line) { +Line3d triangulate_line_with_infinite_line(const Line2d &l1, + const CameraView &view1, + const InfiniteLine3d &inf_line) { InfiniteLine3d ray1_start = InfiniteLine3d(view1.pose.center(), view1.ray_direction(l1.start)); V3D pstart = inf_line.project_to_infinite_line(ray1_start); @@ -321,9 +322,11 @@ Line3d triangulate_with_infinite_line(const Line2d &l1, const CameraView &view1, // Asymmetric perspective to (view1, l1) // Triangulation with a known point -Line3d triangulate_with_one_point(const Line2d &l1, const CameraView &view1, - const Line2d &l2, const CameraView &view2, - const V3D &point) { +Line3d triangulate_line_with_one_point(const Line2d &l1, + const CameraView &view1, + const Line2d &l2, + const CameraView &view2, + const V3D &point) { // project point onto plane 1 V3D n1 = getNormalDirection(l1, view1); V3D C1 = view1.pose.center(); @@ -383,9 +386,11 @@ Line3d triangulate_with_one_point(const Line2d &l1, const CameraView &view1, // Asymmetric perspective to (view1, l1) // Triangulation with known direction -Line3d triangulate_with_direction(const Line2d &l1, const CameraView &view1, - const Line2d &l2, const CameraView &view2, - const V3D &direction) { +Line3d triangulate_line_with_direction(const Line2d &l1, + const CameraView &view1, + const Line2d &l2, + const CameraView &view2, + const V3D &direction) { // Step 1: project direction onto plane 1 V3D n1 = getNormalDirection(l1, view1); V3D direc = direction - (n1.dot(direction)) * n1; diff --git a/limap/triangulation/functions.h b/limap/triangulation/functions.h index dcc5309a..1c12b6a2 100644 --- a/limap/triangulation/functions.h +++ b/limap/triangulation/functions.h @@ -28,9 +28,8 @@ double compute_epipolar_IoU(const Line2d &l1, const CameraView &view1, const Line2d &l2, const CameraView &view2); // point triangulation -std::pair point_triangulation(const V2D &p1, const CameraView &view1, - const V2D &p2, - const CameraView &view2); +std::pair triangulate_point(const V2D &p1, const CameraView &view1, + const V2D &p2, const CameraView &view2); Eigen::Matrix3d point_triangulation_covariance(const V2D &p1, const CameraView &view1, @@ -38,8 +37,8 @@ point_triangulation_covariance(const V2D &p1, const CameraView &view1, const Eigen::Matrix4d &covariance); // Triangulating endpoints for triangulation -Line3d triangulate_endpoints(const Line2d &l1, const CameraView &view1, - const Line2d &l2, const CameraView &view2); +Line3d triangulate_line_by_endpoints(const Line2d &l1, const CameraView &view1, + const Line2d &l2, const CameraView &view2); // Asymmetric perspective to (view1, l1) // Triangulation by plane intersection @@ -54,24 +53,29 @@ M6D line_triangulation_covariance(const Line2d &l1, const CameraView &view1, // Asymmetric perspective to (view1, l1) // Algebraic line triangulation -Line3d triangulate(const Line2d &l1, const CameraView &view1, const Line2d &l2, - const CameraView &view2); +Line3d triangulate_line(const Line2d &l1, const CameraView &view1, + const Line2d &l2, const CameraView &view2); // unproject endpoints with known infinite line -Line3d triangulate_with_infinite_line(const Line2d &l1, const CameraView &view1, - const InfiniteLine3d &inf_line); +Line3d triangulate_line_with_infinite_line(const Line2d &l1, + const CameraView &view1, + const InfiniteLine3d &inf_line); // Asymmetric perspective to (view1, l1) // Triangulation with a known point -Line3d triangulate_with_one_point(const Line2d &l1, const CameraView &view1, - const Line2d &l2, const CameraView &view2, - const V3D &point); +Line3d triangulate_line_with_one_point(const Line2d &l1, + const CameraView &view1, + const Line2d &l2, + const CameraView &view2, + const V3D &point); // Asymmetric perspective to (view1, l1) // Triangulation with known direction -Line3d triangulate_with_direction(const Line2d &l1, const CameraView &view1, - const Line2d &l2, const CameraView &view2, - const V3D &direction); +Line3d triangulate_line_with_direction(const Line2d &l1, + const CameraView &view1, + const Line2d &l2, + const CameraView &view2, + const V3D &direction); } // namespace triangulation diff --git a/limap/triangulation/triangulation.py b/limap/triangulation/triangulation.py index 4aef5f37..754f6e08 100644 --- a/limap/triangulation/triangulation.py +++ b/limap/triangulation/triangulation.py @@ -60,7 +60,7 @@ def compute_epipolar_IoU(l1, view1, l2, view2): return _tri.compute_epipolar_IoU(l1, view1, l2, view2) -def point_triangulation(p1, view1, p2, view2): +def triangulate_point(p1, view1, p2, view2): """ Two-view point triangulation (mid-point) @@ -72,10 +72,10 @@ def point_triangulation(p1, view1, p2, view2): Returns: point3d (:class:`np.array` of shape (3,)) """ - return _tri.point_triangulation(p1, view1, p2, view2) + return _tri.triangulate_point(p1, view1, p2, view2) -def triangulate_endpoints(l1, view1, l2, view2): +def triangulate_line_by_endpoints(l1, view1, l2, view2): """ Two-view triangulation of lines with point triangulation \ on both endpoints (assuming correspondences) @@ -88,10 +88,10 @@ def triangulate_endpoints(l1, view1, l2, view2): Returns: line3d (:class:`limap.base.Line3d`) """ - return _tri.triangulate_endpoints(l1, view1, l2, view2) + return _tri.triangulate_line_by_endpoints(l1, view1, l2, view2) -def triangulate(l1, view1, l2, view2): +def triangulate_line(l1, view1, l2, view2): """ Two-view triangulation of lines by ray-plane intersection @@ -103,10 +103,10 @@ def triangulate(l1, view1, l2, view2): Returns: line3d (:class:`limap.base.Line3d`) """ - return _tri.triangulate(l1, view1, l2, view2) + return _tri.triangulate_line(l1, view1, l2, view2) -def triangulate_with_one_point(l1, view1, l2, view2, p): +def triangulate_line_with_one_point(l1, view1, l2, view2, p): """ Two-view triangulation of lines with a known 3D point on the line @@ -119,10 +119,10 @@ def triangulate_with_one_point(l1, view1, l2, view2, p): Returns: line3d (:class:`limap.base.Line3d`) """ - return _tri.triangulate_with_one_point(l1, view1, l2, view2, p) + return _tri.triangulate_line_with_one_point(l1, view1, l2, view2, p) -def triangulate_with_direction(l1, view1, l2, view2, direc): +def triangulate_line_with_direction(l1, view1, l2, view2, direc): """ Two-view triangulation of lines with known 3D line direction @@ -135,4 +135,4 @@ def triangulate_with_direction(l1, view1, l2, view2, direc): Returns: line3d (:class:`limap.base.Line3d`) """ - return _tri.triangulate_with_direction(l1, view1, l2, view2, direc) + return _tri.triangulate_line_with_direction(l1, view1, l2, view2, direc) diff --git a/runners/7scenes/localization.py b/runners/7scenes/localization.py index 660f8299..4775df5a 100644 --- a/runners/7scenes/localization.py +++ b/runners/7scenes/localization.py @@ -22,7 +22,7 @@ run_hloc_7scenes, ) -import limap.runners as _runners +import limap.runners as runners import limap.util.config as cfgutils import limap.util.io as limapio @@ -120,7 +120,7 @@ def parse_config(): def main(): cfg, args = parse_config() - cfg = _runners.setup(cfg) + cfg = runners.setup(cfg) # outputs is for localization-related results outputs = Path(cfg["output_dir"]) / "localization" @@ -183,7 +183,7 @@ def main(): finaltracks_dir = os.path.join(cfg["output_dir"], cfg["output_folder"]) if not cfg["skip_exists"] or not os.path.exists(finaltracks_dir): logger.info("Running LIMAP triangulation...") - linetracks_db = _runners.line_triangulation( + linetracks_db = runners.line_triangulation( cfg, imagecols_train, neighbors=neighbors, ranges=ranges ) else: @@ -206,7 +206,7 @@ def main(): ) for id in train_ids } - linetracks_db = _runners.line_fitnmerge( + linetracks_db = runners.line_fitnmerge( cfg, imagecols_train, depths, neighbors, ranges ) else: @@ -240,7 +240,7 @@ def main(): hloc_logs = pickle.load(f) point_correspondences = {} for qid in query_ids: - p2ds, p3ds, inliers = _runners.get_hloc_keypoints_from_log( + p2ds, p3ds, inliers = runners.get_hloc_keypoints_from_log( hloc_logs, img_id_to_name[qid], ref_sfm ) point_correspondences[qid] = { @@ -250,7 +250,7 @@ def main(): } # can return final_poses - _runners.line_localization( + runners.hybrid_localization( cfg, imagecols_train, imagecols_query, diff --git a/runners/cambridge/localization.py b/runners/cambridge/localization.py index 4339ddc7..495aa1cc 100644 --- a/runners/cambridge/localization.py +++ b/runners/cambridge/localization.py @@ -20,7 +20,7 @@ undistort_and_resize, ) -import limap.runners as _runners +import limap.runners as runners import limap.util.config as cfgutils import limap.util.io as limapio @@ -116,7 +116,7 @@ def parse_config(): def main(): cfg, args = parse_config() - cfg = _runners.setup(cfg) + cfg = runners.setup(cfg) scene_id = os.path.basename(cfg["vsfm_path"]) # outputs is for localization-related results @@ -185,7 +185,7 @@ def main(): finaltracks_dir = os.path.join(cfg["output_dir"], "finaltracks") if not cfg["skip_exists"] or not os.path.exists(finaltracks_dir): logger.info("Running LIMAP triangulation...") - linetracks_db = _runners.line_triangulation( + linetracks_db = runners.line_triangulation( cfg, imagecols_train, neighbors=neighbors, ranges=ranges ) else: @@ -222,7 +222,7 @@ def main(): hloc_logs = pickle.load(f) point_correspondences = {} for qid in query_ids: - p2ds, p3ds, inliers = _runners.get_hloc_keypoints_from_log( + p2ds, p3ds, inliers = runners.get_hloc_keypoints_from_log( hloc_logs, hloc_name_dict[qid], ref_sfm ) point_correspondences[qid] = { @@ -232,7 +232,7 @@ def main(): } # can return final_poses - _runners.line_localization( + runners.hybrid_localization( cfg, imagecols_train, imagecols_query, diff --git a/runners/cambridge/utils.py b/runners/cambridge/utils.py index 30ebeb52..830ef1a4 100644 --- a/runners/cambridge/utils.py +++ b/runners/cambridge/utils.py @@ -86,12 +86,12 @@ def get_scene_info(vsfm_path, imagecols, query_images): def undistort_and_resize(cfg, imagecols, logger=None): import cv2 - import limap.runners as _runners + import limap.runners as runners # undistort images logger.info("Performing undistortion...") if not imagecols.IsUndistorted(): - imagecols = _runners.undistort_images( + imagecols = runners.undistort_images( imagecols, os.path.join(cfg["output_dir"], cfg["undistortion_output_dir"]), skip_exists=cfg["load_undistort"] or cfg["skip_exists"], diff --git a/runners/inloc/localization.py b/runners/inloc/localization.py index 3ce7a6c6..bea0ab1a 100644 --- a/runners/inloc/localization.py +++ b/runners/inloc/localization.py @@ -198,7 +198,7 @@ def main(): "inliers": inliers, } - final_poses = runners.line_localization( + final_poses = runners.hybrid_localization( cfg, imagecols_train, imagecols_query, diff --git a/runners/pointline_association.py b/runners/pointline_association.py index 880b8515..6e529ed2 100644 --- a/runners/pointline_association.py +++ b/runners/pointline_association.py @@ -95,7 +95,6 @@ def pointline_association(cfg, input_folder, output_folder, colmap_folder): cfg_associator = optimize.GlobalAssociatorConfig( cfg["global_pl_association"] ) - # cfg_associator.solver_options.logging_type = _ceresbase.LoggingType.STDOUT associator = optimize.GlobalAssociator(cfg_associator) associator.InitImagecols(imagecols) associator.InitPointTracks(pointtracks) diff --git a/scripts/format/python.sh b/scripts/format/python.sh index adb4706f..7dd575c3 100755 --- a/scripts/format/python.sh +++ b/scripts/format/python.sh @@ -23,4 +23,4 @@ echo "Formatting ${num_files} files" # shellcheck disable=SC2086 ruff format --config ${root_folder}/ruff.toml ${all_files} -ruff check --fix --config ${root_folder}/ruff.toml ${all_files} +ruff check --config ${root_folder}/ruff.toml ${all_files} diff --git a/visualize_3d_lines.py b/visualize_3d_lines.py index b1499484..cf3e4c2b 100644 --- a/visualize_3d_lines.py +++ b/visualize_3d_lines.py @@ -1,6 +1,6 @@ import os -import limap.base as _base +import limap.base as base import limap.util.io as limapio import limap.visualize as limapvis @@ -113,7 +113,7 @@ def main(args): not args.imagecols.endswith(".npy") ): raise ValueError(f"Error! Input file {args.imagecols} is not valid") - imagecols = _base.ImageCollection( + imagecols = base.ImageCollection( limapio.read_npy(args.imagecols).item() ) vis_reconstruction(