corrected cpp traj dump, solved pybind bug

rvp-group · Nov 21, 2024 · 97603a1 · 97603a1
1 parent b3260b6
commit 97603a1
Show file tree

Hide file tree

Showing 6 changed files with 53 additions and 34 deletions.
diff --git a/mad_icp/apps/cpp_runners/bin_runner.cpp b/mad_icp/apps/cpp_runners/bin_runner.cpp
@@ -255,7 +255,13 @@ void writeTransformedPose(std::ofstream& estimate_file_buf,
   const Eigen::Matrix4d base_to_world = lidar_to_base * lidar_to_world * lidar_to_base.inverse();
   for (int row = 0; row < 3; ++row) {
     for (int col = 0; col < 4; ++col) {
-      estimate_file_buf << base_to_world(row, col) << " ";
+      estimate_file_buf << base_to_world(row, col);
+      if (col < 3) {
+        estimate_file_buf << " ";
+      }
+    }
+    if (row < 2) {
+      estimate_file_buf << " ";
     }
   }
   estimate_file_buf << std::endl;

diff --git a/mad_icp/apps/mad_icp.py b/mad_icp/apps/mad_icp.py
@@ -46,8 +46,7 @@
 from mad_icp.configurations.datasets.dataset_configurations import DatasetConfiguration_lut
 from mad_icp.configurations.mad_params import MADConfiguration_lut
 # binded vectors and odometry
-from mad_icp.src.pybind.pyvector import VectorEigen3d
-from mad_icp.src.pybind.pypeline import Pipeline
+from mad_icp.src.pybind.pypeline import Pipeline, VectorEigen3d
 
 
 console = Console()

diff --git a/mad_icp/configurations/datasets/dataset_configurations.py b/mad_icp/configurations/datasets/dataset_configurations.py
@@ -49,9 +49,9 @@
     "deskew": False,
     "apply_correction": True,
     "lidar_to_base": [
-        [-0.7071, -0.7071, 0, -0.0843],
-        [0.7071, -0.7071, 0, -0.0250],
-        [0, 0, 1, 0.0502],
+        [4.276802385584e-04, -9.999672484946e-01, -8.084491683471e-03, -1.198459927713e-02],
+        [-7.210626507497e-03, 8.081198471645e-03, -9.999413164504e-01, -5.403984729748e-02],
+        [9.999738645903e-01, 4.859485810390e-04, -7.206933692422e-03, -2.921968648686e-01],
         [0, 0, 0, 1]
     ]
 }

diff --git a/mad_icp/configurations/datasets/kitti.cfg b/mad_icp/configurations/datasets/kitti.cfg
@@ -5,7 +5,7 @@ sensor_hz : 10
 deskew : False
 apply_correction: True
 lidar_to_base: 
-  - [-0.7071, -0.7071, 0, -0.0843]
-  - [0.7071, -0.7071, 0, -0.0250]
-  - [0, 0, 1, 0.0502]
+  - [4.276802385584e-04, -9.999672484946e-01, -8.084491683471e-03, -1.198459927713e-02]
+  - [-7.210626507497e-03, 8.081198471645e-03, -9.999413164504e-01, -5.403984729748e-02]
+  - [9.999738645903e-01, 4.859485810390e-04, -7.206933692422e-03, -2.921968648686e-01]
   - [0, 0, 0, 1]
diff --git a/mad_icp/src/pybind/pypeline.cpp b/mad_icp/src/pybind/pypeline.cpp
@@ -27,35 +27,49 @@
 // POSSIBILITY OF SUCH DAMAGE.
 
 // pybind11
+#include <pybind11/chrono.h>
+#include <pybind11/complex.h>
+#include <pybind11/eigen.h>
+#include <pybind11/functional.h>
+#include <pybind11/numpy.h>
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/stl_bind.h>
+
+// std stuff
+#include <Eigen/Core>
+#include <memory>
+#include <vector>
 
-#include "odometry/pipeline.h"
 #include "eigen_stl_bindings.h"
+#include "odometry/pipeline.h"
+
+PYBIND11_MAKE_OPAQUE(std::vector<Eigen::Vector3d>);
 
 namespace py11 = pybind11;
 using namespace py11::literals;
 
 PYBIND11_MODULE(pypeline, m) {
-  auto pipeline = py11::class_<Pipeline>(m, "Pipeline")
-                    .def(py11::init<double, bool, double, double, double, double, double, int, int, bool>(),
-                         py11::arg("sensor_hz"),
-                         py11::arg("deskew"),
-                         py11::arg("b_max"),
-                         py11::arg("rho_ker"),
-                         py11::arg("p_th"),
-                         py11::arg("b_min"),
-                         py11::arg("b_ratio"),
-                         py11::arg("num_keyframes"),
-                         py11::arg("num_threads"),
-                         py11::arg("realtime"))
-                    .def("currentPose", &Pipeline::currentPose)
-                    .def("trajectory", &Pipeline::trajectory)
-                    .def("keyframePose", &Pipeline::keyframePose)
-                    .def("isInitialized", &Pipeline::isInitialized)
-                    .def("isMapUpdated", &Pipeline::isMapUpdated)
-                    .def("currentID", &Pipeline::currentID)
-                    .def("keyframeID", &Pipeline::keyframeID)
-                    .def("modelLeaves", &Pipeline::modelLeaves)
-                    .def("currentLeaves", &Pipeline::currentLeaves)
-                    .def("compute", &Pipeline::compute);
-}
+  auto vector3dvector = pybind_eigen_vector_of_vector<Eigen::Vector3d>(
+      m, "VectorEigen3d", "std::vector<Eigen::Vector3d>",
+      py11::py_array_to_vectors_double<Eigen::Vector3d>);
+
+  auto pipeline =
+      py11::class_<Pipeline>(m, "Pipeline")
+          .def(py11::init<double, bool, double, double, double, double, double,
+                          int, int, bool>(),
+               py11::arg("sensor_hz"), py11::arg("deskew"), py11::arg("b_max"),
+               py11::arg("rho_ker"), py11::arg("p_th"), py11::arg("b_min"),
+               py11::arg("b_ratio"), py11::arg("num_keyframes"),
+               py11::arg("num_threads"), py11::arg("realtime"))
+          .def("currentPose", &Pipeline::currentPose)
+          .def("trajectory", &Pipeline::trajectory)
+          .def("keyframePose", &Pipeline::keyframePose)
+          .def("isInitialized", &Pipeline::isInitialized)
+          .def("isMapUpdated", &Pipeline::isMapUpdated)
+          .def("currentID", &Pipeline::currentID)
+          .def("keyframeID", &Pipeline::keyframeID)
+          .def("modelLeaves", &Pipeline::modelLeaves)
+          .def("currentLeaves", &Pipeline::currentLeaves)
+          .def("compute", &Pipeline::compute);
+}
diff --git a/pyproject.toml b/pyproject.toml
@@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build"
 
 [project]
 name = "mad-icp"
-version = "0.0.7"
+version = "0.0.8"
 description = "It Is All About Matching Data -- Robust and Informed LiDAR Odometry"
 readme = "README.md"
 authors = [