another module

Signed-off-by: Christian Henkel <[email protected]>

.gitignore

@@ -55,3 +55,6 @@ roadmaps/gsorm/build/*
 roadmaps/benchmark_examples/
 roadmaps/benchmark_plots/benchmark.png
 roadmaps/benchmark_plots/benchmark_*.png
+
+# build folder for cvd
+roadmaps/cvd/build/*

roadmaps/benchmark.py

@@ -401,6 +401,41 @@ def __init__(self,
 #         self._set_graph(g)
 
 
+class CVD(RoadmapToTest):
+    def __init__(self,
+                 map_fname: str,
+                 rng: Random,
+                 roadmap_specific_kwargs):
+        super().__init__(map_fname, rng, roadmap_specific_kwargs)
+        from roadmaps.cvd.build.libcvd import CVD
+        from roadmaps.var_odrm_torch.var_odrm_torch import (
+            make_graph_and_flann, optimize_poses)
+
+        # prepare args
+        cvd_kwargs = self.roadmap_specific_kwargs.copy()
+
+        # run cvd
+        cvd = CVD()
+        nodes, runtime_points = cvd.run(
+            mapFile=map_fname,
+            **cvd_kwargs,
+            plot=False,
+        )
+        pos = torch.Tensor(nodes) / roadmap_specific_kwargs["resolution"]
+        # swap x and y
+        pos = pos[:, [1, 0]]
+        pos.requires_grad = True
+        n = pos.shape[0]
+        g, _ = make_graph_and_flann(pos, self.map_img, n, rng)
+
+        # swap x and y
+        nx.set_node_attributes(g,
+                               {i: (p[1],
+                                    p[0]) for i, p in nx.get_node_attributes(
+                                   g, POS).items()}, POS)
+        self._set_graph(g)
+
+
 class SPARS2(RoadmapToTest):
     def __init__(self,
                  map_fname: str,
@@ -665,6 +700,34 @@ def run():
     df = pd.DataFrame()
     ns = [500, 1200, 2000]
     trials = [
+        (CVD, {
+            'DA': 0.14,
+            'DB': 0.06,
+            'f': 0.035,
+            'k': 0.065,
+            'delta_t': 1.0,
+            'iterations': 10000,
+            'target_n': ns[0],
+            'plot': True,
+        }),
+        (CVD, {
+            'DA': 0.14,
+            'DB': 0.06,
+            'f': 0.035,
+            'k': 0.065,
+            'delta_t': 1.0,
+            'iterations': 10000,
+            'target_n': ns[1],
+        }),
+        (CVD, {
+            'DA': 0.14,
+            'DB': 0.06,
+            'f': 0.035,
+            'k': 0.065,
+            'delta_t': 1.0,
+            'iterations': 10000,
+            'target_n': ns[2],
+        }),
         (GSRM, {
             'DA': 0.14,
             'DB': 0.06,

roadmaps/cvd/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.5)
+project(cvd)
+
+set(CMAKE_CXX_STANDARD 14)
+# set(CMAKE_CXX_STANDARD_REQUIRED ON)
+# set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_PREFIX_PATH "$ENV{HOME}/bin/libtorch/share/cmake/Torch")
+
+# Set the python version
+set(PYTHON_VERSION 310)
+set(PYTHON_DOT_VERSION 3.10)
+set(PYTHON_INCLUDE /usr/include/python${PYTHON_DOT_VERSION})
+set(PYTHON_LIBRARY /usr/lib/python${PYTHON_DOT_VERSION}/config-${PYTHON_DOT_VERSION}m-x86_64-linux-gnu/)
+
+FIND_PACKAGE(Boost 1.71.0 REQUIRED COMPONENTS 
+    program_options 
+    python${PYTHON_VERSION}
+)
+
+# FIND_PACKAGE(Torch REQUIRED)
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}")
+find_package( OpenCV REQUIRED )
+
+include_directories(
+  src
+  lodepng
+  ${OpenCV_INCLUDE_DIRS}
+  ${PYTHON_INCLUDE}
+)
+
+# the python module
+SET(cvd cvd)
+
+add_library(${cvd} SHARED 
+  src/${cvd}.cpp
+  lodepng/lodepng.cpp
+)
+
+TARGET_LINK_LIBRARIES(
+  ${cvd} 
+  ${Boost_LIBRARIES}
+  ${OpenCV_LIBS}
+)

roadmaps/cvd/README.md

@@ -0,0 +1,16 @@
+# Centroidal Voronoi tessellation (CVD)
+
+## Requirements
+
+- Boost 1.71
+- lodepng (checkout in folder)
+- opencv 4.2.0
+
+## build
+
+```
+mkdir build
+cd build
+cmake ..
+make
+```

roadmaps/cvd/demo.py

@@ -0,0 +1,21 @@
+from matplotlib import pyplot as plt
+import matplotlib.image as mpimg
+import numpy as np
+
+from roadmaps.cvd.build.libcvd import CVD
+
+if __name__ == '__main__':
+    img_path = "roadmaps/odrm/odrm_eval/maps/plain.png"
+
+    cvd = CVD()
+    (nodes, t) = cvd.run(img_path, 100)
+    img = mpimg.imread(img_path)
+    nodes = np.array(nodes)
+
+    fig = plt.figure()
+    plt.imshow(img)
+    plt.plot(nodes[:, 1], nodes[:, 0], 'r.')
+
+    plt.axis('scaled')  # Ensure aspect ratio is maintained
+    plt.show()
+

roadmaps/cvd/src/cvd.cpp

@@ -0,0 +1,117 @@
+#include <boost/program_options.hpp>
+#include <boost/python.hpp>
+
+#include <iostream>
+
+#include "lodepng.h"
+// #include <torch/torch.h>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <chrono>
+using namespace std::chrono;
+
+class CVD
+{
+public:
+  CVD()
+  {
+  }
+
+  boost::python::tuple run(
+    std::string mapFile, int n_nodes, int seed = 1)
+  {
+    std::string example_folder = "roadmaps/cvd/examples/";
+
+    // seed opencv rng
+    cv::RNG rng(seed);
+
+    // Load PNG image
+    std::vector<unsigned char> image; // the raw pixels
+    unsigned width, height;
+    unsigned error = lodepng::decode(image, width, height, mapFile, LCT_GREY, 8);
+    if (error) {
+      std::cerr << "decoder error " << error << ": " << lodepng_error_text(error) << std::endl;
+      return {};
+    }
+    std::cout << "Loaded png: " << width << "x" << height << std::endl;
+
+    // Initialize Points
+    auto start = high_resolution_clock::now();
+    std::vector<cv::Point> nodes;
+    while (nodes.size() < n_nodes) {
+      float x = rng.uniform(0., (float) width);
+      float y = rng.uniform(0., (float) height);
+      if (image[(int) y * width + (int) x] == 255) {
+        nodes.push_back(cv::Point(x, y));
+      }
+    }
+
+    // Make Voronoi diagram
+    cv::Mat mask = cv::Mat::zeros(height, width, CV_8U);
+    for (int i = 0; i < mask.rows; i++) {
+      for (int j = 0; j < mask.cols; j++) {
+        if (image[i * width + j] == 255) {
+          mask.at<uchar>(i, j) = 255;
+        }
+      }
+    }
+    cv::Subdiv2D subdiv(cv::Rect(50, 50, 910, 910));
+    for (auto node : nodes) {
+      subdiv.insert(node);
+    }
+
+    // Draw Voronoi diagram
+    cv::Mat img_voronoi = cv::Mat::zeros(mask.size(), CV_8UC3);
+    std::vector<std::vector<cv::Point2f>> facets;
+    std::vector<cv::Point2f> centers;
+    subdiv.getVoronoiFacetList(std::vector<int>(), facets, centers);
+    for (size_t i = 0; i < facets.size(); i++) {
+      std::vector<cv::Point> facet;
+      for (size_t j = 0; j < facets[i].size(); j++) {
+        facet.push_back(facets[i][j]);
+      }
+      cv::fillConvexPoly(img_voronoi, facet, cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), 8, 0);
+    }
+    cv::imwrite(example_folder + "voronoi.png", img_voronoi);
+
+    auto stop = high_resolution_clock::now();
+    auto duration = duration_cast<microseconds>(stop - start);
+
+    float duration_total_ms = static_cast<float>(duration.count()) / 1000.;
+    std::cout << "Total time taken by function: " <<
+      duration_total_ms << " ms" << std::endl;
+
+    // if (plot) {
+    //   // draw middles
+    //   cv::Mat middles_layer = cv::Mat::zeros(mask.size(), CV_8U);
+    //   for (auto middle : middles) {
+    //     cv::circle(middles_layer, middle, 1, cv::Scalar(255), -1);
+    //   }
+
+    //   // overlay middles on spots
+    //   cv::Mat spots_middles_layer = cv::Mat::zeros(mask.size(), CV_8UC3);
+    //   cv::cvtColor(spots_layer, spots_middles_layer, cv::COLOR_GRAY2BGR);
+    //   cv::cvtColor(middles_layer, middles_layer, cv::COLOR_GRAY2BGR);
+    //   cv::addWeighted(spots_middles_layer, 0.3, middles_layer, 0.7, 0, spots_middles_layer);
+    //   cv::imwrite(example_folder + "spots_middles_layer.png", spots_middles_layer);
+    // }
+
+    boost::python::list points;
+    for (auto node : nodes) {
+      points.append(boost::python::make_tuple(node.x, node.y));
+    }
+    return boost::python::make_tuple(points, duration_total_ms);
+  }
+};
+
+BOOST_PYTHON_MODULE(libcvd)
+{
+  using namespace boost::python;
+  namespace bp = boost::python;
+  class_<CVD>("CVD", init<>())
+  .def(
+    "run", &CVD::run,
+    (bp::arg("mapFile"), bp::arg("n_nodes"), bp::arg("seed") = 0)
+  )
+  ;
+}