diff --git a/mad_icp/apps/cpp_runners/bin_runner.cpp b/mad_icp/apps/cpp_runners/bin_runner.cpp index e2108dd..be42066 100644 --- 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 index 2b3233b..54ca721 100755 --- 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 index 2d3681f..c2bc566 100644 --- 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 index 1206567..35afc59 100755 --- 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 index 058f571..a6178b7 100755 --- a/mad_icp/src/pybind/pypeline.cpp +++ b/mad_icp/src/pybind/pypeline.cpp @@ -27,35 +27,49 @@ // POSSIBILITY OF SUCH DAMAGE. // pybind11 +#include +#include +#include +#include +#include #include +#include +#include + +// std stuff +#include +#include +#include -#include "odometry/pipeline.h" #include "eigen_stl_bindings.h" +#include "odometry/pipeline.h" + +PYBIND11_MAKE_OPAQUE(std::vector); namespace py11 = pybind11; using namespace py11::literals; PYBIND11_MODULE(pypeline, m) { - auto pipeline = py11::class_(m, "Pipeline") - .def(py11::init(), - 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( + m, "VectorEigen3d", "std::vector", + py11::py_array_to_vectors_double); + + auto pipeline = + py11::class_(m, "Pipeline") + .def(py11::init(), + 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); +} \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index 95b2526..92cff25 100644 --- 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 = [