for wgs84 coords in PolylineRuler, convert lla2enu only once (#18)

* update headers

* update build script

* update linux image

* misc updates

* add cheapRuler

* init cheap ruler

* more cheap ruler bindings, tests

* better performance for wgs84 coords

* more binding, should be faster

* update docs

* lint code

* better

* fix

* update headers (synced)

---------

Co-authored-by: TANG ZHIXIONG <zhixiong.tang@momenta.ai>

Author: district10

Makefile

@@ -1,3 +1,6 @@
+PROJECT_SOURCE_DIR ?= $(abspath ./)
+PROJECT_NAME ?= $(shell basename $(PROJECT_SOURCE_DIR))
+
 all:
 	@echo nothing special
 
@@ -23,7 +26,7 @@ docs_serve:
 	mkdocs serve -a 0.0.0.0:8088
 
 DOCKER_TAG_WINDOWS ?= ghcr.io/cubao/build-env-windows-x64:v0.0.1
-DOCKER_TAG_LINUX ?= ghcr.io/cubao/build-env-manylinux2014-x64:v0.0.1
+DOCKER_TAG_LINUX ?= ghcr.io/cubao/build-env-manylinux2014-x64:v0.0.3
 DOCKER_TAG_MACOS ?= ghcr.io/cubao/build-env-macos-arm64:v0.0.1
 
 test_in_win:
@@ -33,6 +36,16 @@ test_in_mac:
 test_in_linux:
 	docker run --rm -w `pwd` -v `pwd`:`pwd` -v `pwd`/build/linux:`pwd`/build -it $(DOCKER_TAG_LINUX) bash
 
+DEV_CONTAINER_NAME ?= $(USER)_$(subst /,_,$(PROJECT_NAME)____$(PROJECT_SOURCE_DIR))
+DEV_CONTAINER_IMAG ?= $(DOCKER_TAG_LINUX)
+test_in_container:
+test_in_dev_container:
+	docker ps | grep $(DEV_CONTAINER_NAME) \
+		&& docker exec -it $(DEV_CONTAINER_NAME) bash \
+		|| docker run --rm --name $(DEV_CONTAINER_NAME) \
+			--network host --security-opt seccomp=unconfined \
+			-v `pwd`:`pwd` -w `pwd` -it $(DEV_CONTAINER_IMAG) bash
+
 PYTHON ?= python3
 python_install:
 	$(PYTHON) setup.py install
@@ -62,11 +75,13 @@ python_build_py39:
 	PYTHON=python conda run --no-capture-output -n py39 make python_build
 python_build_py310:
 	PYTHON=python conda run --no-capture-output -n py310 make python_build
-python_build_all: python_build_py36 python_build_py37 python_build_py38 python_build_py39 python_build_py310
+python_build_py311:
+	PYTHON=python conda run --no-capture-output -n py311 make python_build
+python_build_all: python_build_py36 python_build_py37 python_build_py38 python_build_py39 python_build_py310 python_build_py311
 python_build_all_in_linux:
-	docker run --rm -w `pwd` -v `pwd`:`pwd` -v `pwd`/build/win:`pwd`/build -it $(DOCKER_TAG_LINUX) make python_build_all
+	docker run --rm -w `pwd` -v `pwd`:`pwd` -v `pwd`/build/linux:`pwd`/build -it $(DOCKER_TAG_LINUX) make python_build_all
 	make repair_wheels && rm -rf dist/*.whl && mv wheelhouse/*.whl dist && rm -rf wheelhouse
-python_build_all_in_macos: python_build_py38 python_build_py39 python_build_py310
+python_build_all_in_macos: python_build_py38 python_build_py39 python_build_py310 python_build_py311
 python_build_all_in_windows: python_build_all
 
 repair_wheels:
@@ -82,14 +97,15 @@ tar.gz:
 	tar -cvz --exclude .git -f ../polyline-ruler.tar.gz .
 	ls -alh ../polyline-ruler.tar.gz
 
-SYNC_OUTPUT_DIR := headers/include/cubao
+SYNC_OUTPUT_DIR ?= headers/include/cubao
 sync_headers:
 	mkdir -p $(SYNC_OUTPUT_DIR)
 	cp src/cheap_ruler.hpp $(SYNC_OUTPUT_DIR)
 	cp src/crs_transform.hpp $(SYNC_OUTPUT_DIR)
 	cp src/cubao_inline.hpp $(SYNC_OUTPUT_DIR)
 	cp src/eigen_helpers.hpp $(SYNC_OUTPUT_DIR)
 	cp src/polyline_ruler.hpp $(SYNC_OUTPUT_DIR)
+	cp src/pybind11_cheap_ruler.hpp $(SYNC_OUTPUT_DIR)
 	cp src/pybind11_crs_transform.hpp $(SYNC_OUTPUT_DIR)
 	cp src/pybind11_polyline_ruler.hpp $(SYNC_OUTPUT_DIR)
 

docs/about/release-notes.md

@@ -10,6 +10,10 @@ To upgrade `polyline-ruler` to the latest version, use pip:
 pip install -U polyline-ruler
 ```
 
+## Version 0.0.5 (2023-06-23)
+
+*   Faster pointOnLine by lla2enu hard copy once
+
 ## Version 0.0.4 (2023-03-11)
 
 *   Misc updates

headers

@@ -122,7 +122,7 @@ def build_extension(self, ext):
 # logic and declaration, and simpler if you include description/version in a file.
 setup(
     name="polyline_ruler",
-    version="0.0.4",
+    version="0.0.5",
     author="tzx",
     author_email="dvorak4tzx@gmail.com",
     url="https://polyline-ruler.readthedocs.io",

setup.py

@@ -16,14 +16,19 @@
 #include <Eigen/Core>
 
 #include <cassert>
-#include <cmath>
 #include <cstdint>
 #include <limits>
 #include <array>
 #include <tuple>
 #include <utility>
 #include <vector>
 
+#define _USE_MATH_DEFINES
+#include <cmath>
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
 namespace cubao
 {
 using RowVectors = Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>;
@@ -41,14 +46,14 @@ using polygon = RowVectors;
 class CheapRuler
 {
 
+  public:
     // Values that define WGS84 ellipsoid model of the Earth
     static constexpr double RE = 6378.137;            // equatorial radius
     static constexpr double FE = 1.0 / 298.257223563; // flattening
 
     static constexpr double E2 = FE * (2 - FE);
     static constexpr double RAD = M_PI / 180.0;
 
-  public:
     enum Unit
     {
         Kilometers,
@@ -160,7 +165,7 @@ class CheapRuler
     //
     // Returns the bearing between two points in angles.
     //        0
-    //   45         45
+    //   -45         45
     // -90              90
     // -135         135
     //       180
@@ -472,14 +477,15 @@ class CheapRuler
         return point(a[0] + dx * t, a[1] + dy * t, a[2] + dz * t);
     }
 
-  private:
-    double ky;
-    double kx;
-    double kz;
     static double longDiff(double a, double b)
     {
         return std::remainder(a - b, 360);
     }
+
+  private:
+    double ky;
+    double kx;
+    double kz;
 };
 
 } // namespace cheap_ruler

src/cheap_ruler.hpp

@@ -15,6 +15,12 @@
 #include <Eigen/Geometry>
 #include <optional>
 
+#define _USE_MATH_DEFINES
+#include <cmath>
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
 namespace cubao
 {
 

src/crs_transform.hpp

@@ -27,6 +27,7 @@
 
 #include "pybind11_crs_transform.hpp"
 #include "pybind11_polyline_ruler.hpp"
+#include "pybind11_cheap_ruler.hpp"
 
 #define STRINGIFY(x) #x
 #define MACRO_STRINGIFY(x) STRINGIFY(x)
@@ -53,6 +54,7 @@ PYBIND11_MODULE(polyline_ruler, m)
     cubao::bind_crs_transform(tf);
 
     cubao::bind_polyline_ruler(m);
+    cubao::bind_cheap_ruler(m);
 
 #ifdef VERSION_INFO
     m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO);

src/main.cpp

@@ -15,7 +15,6 @@
 #include <optional>
 #include <queue>
 
-#include "cheap_ruler.hpp"
 #include "crs_transform.hpp"
 #include "eigen_helpers.hpp"
 
@@ -224,7 +223,7 @@ struct PolylineRuler
         : polyline_(polyline),                        //
           N_(polyline.rows()),                        //
           is_wgs84_(is_wgs84),                        //
-          k_(is_wgs84 ? CheapRuler::k(polyline(0, 1)) //
+          k_(is_wgs84 ? cheap_ruler_k(polyline(0, 1)) //
                       : Eigen::Vector3d::Ones())
     {
     }
@@ -244,6 +243,7 @@ struct PolylineRuler
     // cache
     mutable std::optional<Eigen::VectorXd> ranges_;
     mutable std::optional<RowVectors> dirs_;
+    mutable std::optional<RowVectors> enus_; // only when is_wgs84==true
 
   public:
     const RowVectors &polyline() const { return polyline_; }
@@ -370,6 +370,49 @@ struct PolylineRuler
         return *dirs_;
     }
 
+  private:
+    const RowVectors &enus() const
+    {
+        assert(is_wgs84_);
+        if (!enus_) {
+            enus_ = __lla2enu(polyline_);
+        }
+        return *enus_;
+    }
+    Eigen::Vector3d __enu2lla(const Eigen::Vector3d &enu) const
+    {
+        return (enu.array() / k_.array()) + Eigen::Array3d(polyline_(0, 0),
+                                                           polyline_(0, 1),
+                                                           polyline_(0, 2));
+    }
+    Eigen::Vector3d __lla2enu(const Eigen::Vector3d &lla) const
+    {
+        return k_.array() * (lla - Eigen::Vector3d(polyline_(0, 0), //
+                                                   polyline_(0, 1), //
+                                                   polyline_(0, 2)))
+                                .array();
+    }
+
+    RowVectors __enu2lla(const RowVectors &enus) const
+    {
+        RowVectors llas = enus;
+        for (int i = 0; i < 3; ++i) {
+            llas.col(i).array() /= k_[i];
+            llas.col(i).array() += polyline_(0, i);
+        }
+        return llas;
+    }
+    RowVectors __lla2enu(const RowVectors &llas) const
+    {
+        RowVectors enus = llas;
+        for (int i = 0; i < 3; ++i) {
+            enus.col(i).array() -= polyline_(0, i);
+            enus.col(i).array() *= k_[i];
+        }
+        return enus;
+    }
+
+  public:
     Eigen::Vector3d dir(int pt_index) const
     {
         return dirs().row(std::min(pt_index, N_ - 2));
@@ -395,8 +438,7 @@ struct PolylineRuler
     Eigen::Vector3d extended_along(double range) const
     {
         auto [i, t] = segment_index_t(range);
-        return interpolate(polyline_.row(i), polyline_.row(i + 1), t,
-                           is_wgs84_);
+        return interpolate(polyline_.row(i), polyline_.row(i + 1), t);
     }
 
     Eigen::Vector3d at(double range) const { return extended_along(range); }
@@ -405,7 +447,7 @@ struct PolylineRuler
     {
         return interpolate(polyline_.row(seg_idx),     //
                            polyline_.row(seg_idx + 1), //
-                           t, is_wgs84_);
+                           t);
     }
 
     std::pair<Eigen::Vector3d, Eigen::Vector3d> arrow(int seg_idx,
@@ -432,7 +474,8 @@ struct PolylineRuler
             xyzs.row(i) = arrow.first;
             dirs.row(i) = arrow.second;
         }
-        return std::make_tuple(std::move(ranges), std::move(xyzs),
+        return std::make_tuple(std::move(ranges), //
+                               std::move(xyzs),   //
                                std::move(dirs));
     }
 
@@ -536,7 +579,8 @@ struct PolylineRuler
     }
     Eigen::Vector3d along(double dist) const
     {
-        return along(polyline_, dist, is_wgs84_);
+        return is_wgs84_ ? __enu2lla(along(enus(), dist))
+                         : along(polyline_, dist);
     }
 
     static double pointToSegmentDistance(const Eigen::Vector3d &p,
@@ -609,7 +653,11 @@ struct PolylineRuler
     std::tuple<Eigen::Vector3d, int, double>
     pointOnLine(const Eigen::Vector3d &p) const
     {
-        return pointOnLine(polyline_, p, is_wgs84_);
+        if (!is_wgs84_) {
+            return pointOnLine(polyline_, p);
+        }
+        auto [enu, i, t] = pointOnLine(enus(), __lla2enu(p));
+        return std::make_tuple(__enu2lla(enu), i, t);
     }
 
     static RowVectors lineSlice(const Eigen::Vector3d &start,
@@ -668,7 +716,10 @@ struct PolylineRuler
     RowVectors lineSlice(const Eigen::Vector3d &start,
                          const Eigen::Vector3d &stop) const
     {
-        return lineSlice(start, stop, polyline_, is_wgs84_);
+        if (!is_wgs84_) {
+            return lineSlice(start, stop, polyline_);
+        }
+        return __enu2lla(lineSlice(__lla2enu(start), __lla2enu(stop), enus()));
     }
 
     static RowVectors lineSliceAlong(double start, double stop,
@@ -709,23 +760,15 @@ struct PolylineRuler
     }
     RowVectors lineSliceAlong(double start, double stop) const
     {
-        return lineSliceAlong(start, stop, polyline_, is_wgs84_);
+        if (!is_wgs84_) {
+            return lineSliceAlong(start, stop, polyline_);
+        }
+        return __enu2lla(lineSliceAlong(start, stop, enus()));
     }
 
     static Eigen::Vector3d interpolate(const Eigen::Vector3d &a,
-                                       const Eigen::Vector3d &b, double t,
-                                       bool is_wgs84 = false)
+                                       const Eigen::Vector3d &b, double t)
     {
-        if (is_wgs84) {
-            RowVectors llas(2, 3);
-            llas.row(0) = a;
-            llas.row(1) = b;
-            auto enus = lla2enu(llas);
-            return enu2lla(interpolate(enus.row(0), enus.row(1), t, !is_wgs84)
-                               .transpose(),
-                           llas.row(0))
-                .row(0);
-        }
         return a + (b - a) * t;
     }
 };
@@ -755,8 +798,9 @@ inline void douglas_simplify(const Eigen::Ref<const RowVectors> &coords,
     douglas_simplify(coords, to_keep, max_index, j, epsilon);
 }
 
-void douglas_simplify_iter(const Eigen::Ref<const RowVectors> &coords,
-                           Eigen::VectorXi &to_keep, const double epsilon)
+inline void douglas_simplify_iter(const Eigen::Ref<const RowVectors> &coords,
+                                  Eigen::VectorXi &to_keep,
+                                  const double epsilon)
 {
     std::queue<std::pair<int, int>> q;
     q.push({0, to_keep.size() - 1});