From 9dcee79253aa1b7e5b804f9dde44b3a264037e83 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 26 Jan 2024 08:34:22 +0100 Subject: [PATCH 01/10] version bump --- appveyor.yml | 2 +- doc/source/doxygen-docs/changelog.md | 3 +++ package.xml | 2 +- version_prefix.txt | 2 +- 4 files changed, 6 insertions(+), 3 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index 0735d47627..618a58a5e4 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,5 @@ # version format -version: 2.11.7-{branch}-build{build} +version: 2.11.8-{branch}-build{build} os: Visual Studio 2019 diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 1675170e84..8d47f65aeb 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,5 +1,8 @@ \page changelog Change Log +# Version 2.11.8: UNRELEASED +(none) + # Version 2.11.7: Released Jan 25th, 2024 - Changes in apps: - carmen2rawlog: Generate valid timestamps. diff --git a/package.xml b/package.xml index 5f3303cd66..3df498f725 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ mrpt2 - 2.11.7 + 2.11.8 Mobile Robot Programming Toolkit (MRPT) version 2.x Jose-Luis Blanco-Claraco diff --git a/version_prefix.txt b/version_prefix.txt index b0836f70e3..8e77878ebf 100644 --- a/version_prefix.txt +++ b/version_prefix.txt @@ -1,4 +1,4 @@ -2.11.7 +2.11.8 # IMPORTANT: This file is parsed by CMake, don't add any comment to # the first line. # This file is used in both Windows and Linux scripts to automatically From 9ebe161696da142146d692414b3a2b01e790a422 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 26 Jan 2024 14:42:01 +0100 Subject: [PATCH 02/10] fix use of obsolete cv_bridge.h header --- doc/source/doxygen-docs/changelog.md | 5 +++-- libs/ros1bridge/CMakeLists.txt | 2 +- libs/ros1bridge/src/image.cpp | 2 +- libs/ros2bridge/CMakeLists.txt | 2 +- libs/ros2bridge/src/image.cpp | 2 +- 5 files changed, 7 insertions(+), 6 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 8d47f65aeb..36f7ce26ea 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,12 +1,13 @@ \page changelog Change Log # Version 2.11.8: UNRELEASED -(none) +- Changes in libraries: + - \ref mrpt_ros2bridge_grp: + - Fix wrong macros leading to including obsolete header ``. # Version 2.11.7: Released Jan 25th, 2024 - Changes in apps: - carmen2rawlog: Generate valid timestamps. -- Changes in libraries: - rosbag2rawlog: Add support for `sensor_msgs/CompressedImage` topics. - BUG FIXES: - mrpt::hwdrivers::CImageGrabber_dc1394 did not mark the right image as present in stereo cameras. diff --git a/libs/ros1bridge/CMakeLists.txt b/libs/ros1bridge/CMakeLists.txt index b5951c0898..8deb2d5fad 100644 --- a/libs/ros1bridge/CMakeLists.txt +++ b/libs/ros1bridge/CMakeLists.txt @@ -54,5 +54,5 @@ if(BUILD_mrpt-ros1bridge) ) target_compile_definitions(ros1bridge PRIVATE ${ROS_DEFINITIONS}) target_compile_definitions(ros1bridge PRIVATE IS_MRPT_ROS1BRIDGE) - target_compile_definitions(ros1bridge PRIVATE cv_bridge_VERSION=${cv_bridge_VERSION_HEX}) + target_compile_definitions(ros1bridge PRIVATE CV_BRIDGE_VERSION=${cv_bridge_VERSION_HEX}) endif() diff --git a/libs/ros1bridge/src/image.cpp b/libs/ros1bridge/src/image.cpp index 868fcd4d14..a040cd2422 100644 --- a/libs/ros1bridge/src/image.cpp +++ b/libs/ros1bridge/src/image.cpp @@ -13,7 +13,7 @@ AUTHOR: Raghavender Sahdev ---------------------------------------------------------------*/ -#if CV_BRIDGE_VERSION <= 0x030400 +#if CV_BRIDGE_VERSION < 0x030400 #include #else #include diff --git a/libs/ros2bridge/CMakeLists.txt b/libs/ros2bridge/CMakeLists.txt index 6465f28a11..d64d4b9bc4 100644 --- a/libs/ros2bridge/CMakeLists.txt +++ b/libs/ros2bridge/CMakeLists.txt @@ -66,5 +66,5 @@ if(BUILD_mrpt-ros2bridge) target_compile_definitions(ros2bridge PRIVATE ${ROS_DEFINITIONS}) target_compile_definitions(ros2bridge PRIVATE IS_MRPT_ROS2BRIDGE) - target_compile_definitions(ros2bridge PRIVATE cv_bridge_VERSION=${cv_bridge_VERSION_HEX}) + target_compile_definitions(ros2bridge PRIVATE CV_BRIDGE_VERSION=${cv_bridge_VERSION_HEX}) endif() diff --git a/libs/ros2bridge/src/image.cpp b/libs/ros2bridge/src/image.cpp index 7e7bdf6b70..9cdf963607 100644 --- a/libs/ros2bridge/src/image.cpp +++ b/libs/ros2bridge/src/image.cpp @@ -15,7 +15,7 @@ #include -#if CV_BRIDGE_VERSION <= 0x030400 +#if CV_BRIDGE_VERSION < 0x030400 #include #else #include From 88b41cded54c781a014ed2ad6f86ebb1d33498eb Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 31 Jan 2024 01:19:03 +0100 Subject: [PATCH 03/10] Add Zenodo DOI --- README.md | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/README.md b/README.md index 4d39e56ec0..222c9f9efa 100644 --- a/README.md +++ b/README.md @@ -10,10 +10,7 @@ - - - -[![Gitpod ready-to-code](https://img.shields.io/badge/Gitpod-ready--to--code-blue?logo=gitpod)](https://gitpod.io/#https://github.com/MRPT/mrpt) +DOI ## 1. Introduction From aee7401404a576b3bd1f312ec8d8de8e8ff75231 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 1 Feb 2024 11:19:16 +0100 Subject: [PATCH 04/10] Add missing observation models --- doc/source/doxygen-docs/changelog.md | 2 + .../mrpt/maps/CVoxelMapOccupancyBase.h | 7 ++-- libs/maps/src/maps/CVoxelMap.cpp | 39 ++++++++++++++++++- libs/maps/src/maps/CVoxelMapOccupancyBase.cpp | 8 ++-- libs/maps/src/maps/CVoxelMapRGB.cpp | 39 ++++++++++++++++++- 5 files changed, 84 insertions(+), 11 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 36f7ce26ea..9cd3e6600d 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -2,6 +2,8 @@ # Version 2.11.8: UNRELEASED - Changes in libraries: + - \ref mrpt_maps_grp: + - Implement missing probabilistic observation models in mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB - \ref mrpt_ros2bridge_grp: - Fix wrong macros leading to including obsolete header ``. diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index 8bf5d6ff46..505ce37b64 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -64,9 +64,10 @@ struct TVoxelMap_LikelihoodOptions : public mrpt::config::CLoadableOptions void writeToStream(mrpt::serialization::CArchive& out) const; void readFromStream(mrpt::serialization::CArchive& in); - /// Speed up the likelihood computation by considering only one out of N - /// rays (default=1) - uint32_t decimation = 1; + /// Speed up the likelihood computation by considering only a maximum of + /// `decimate_up_to` rays. Values <=1 mean use all measurements without + /// decimation. + uint32_t decimate_up_to = 0; /// Minimum occupancy (0,1) for a voxel to be considered occupied. double occupiedThreshold = 0.60; diff --git a/libs/maps/src/maps/CVoxelMap.cpp b/libs/maps/src/maps/CVoxelMap.cpp index dd535b7216..e059b63f70 100644 --- a/libs/maps/src/maps/CVoxelMap.cpp +++ b/libs/maps/src/maps/CVoxelMap.cpp @@ -177,6 +177,41 @@ double CVoxelMap::internal_computeObservationLikelihood( const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const { - THROW_EXCEPTION("TODO"); - return 0; + // build aux 3D pointcloud: + mrpt::maps::CSimplePointsMap pts; + pts.insertObservation(obs, takenFrom); + + if (pts.empty()) return 0; + + double log_lik = .0; // cummulative log likelihoo + + auto lambdaPointLikelihood = [&](float x, float y, float z) { + double probOcc = 0; + const bool voxelExists = getPointOccupancy(x, y, z, probOcc); + if (!voxelExists) return; + log_lik += probOcc; + }; + + const auto& xs = pts.getPointsBufferRef_x(); + const auto& ys = pts.getPointsBufferRef_y(); + const auto& zs = pts.getPointsBufferRef_z(); + + if (pts.size() <= likelihoodOptions.decimate_up_to) + { + for (size_t i = 0; i < pts.size(); ++i) + lambdaPointLikelihood(xs[i], ys[i], zs[i]); + } + else + { + const double delta = + static_cast(pts.size()) / likelihoodOptions.decimate_up_to; + + for (size_t i = 0; i < likelihoodOptions.decimate_up_to; ++i) + { + const auto idx = static_cast(i * delta); + lambdaPointLikelihood(xs[idx], ys[idx], zs[idx]); + } + } + + return log_lik; } diff --git a/libs/maps/src/maps/CVoxelMapOccupancyBase.cpp b/libs/maps/src/maps/CVoxelMapOccupancyBase.cpp index a5ecee3814..db37cc89be 100644 --- a/libs/maps/src/maps/CVoxelMapOccupancyBase.cpp +++ b/libs/maps/src/maps/CVoxelMapOccupancyBase.cpp @@ -89,24 +89,24 @@ void TVoxelMap_RenderingOptions::readFromStream( void TVoxelMap_LikelihoodOptions::loadFromConfigFile( const mrpt::config::CConfigFileBase& c, const std::string& s) { - MRPT_LOAD_CONFIG_VAR(decimation, int, c, s); + MRPT_LOAD_CONFIG_VAR(decimate_up_to, int, c, s); MRPT_LOAD_CONFIG_VAR(occupiedThreshold, double, c, s); } void TVoxelMap_LikelihoodOptions::saveToConfigFile( mrpt::config::CConfigFileBase& c, const std::string& s) const { - MRPT_SAVE_CONFIG_VAR(decimation, c, s); + MRPT_SAVE_CONFIG_VAR(decimate_up_to, c, s); MRPT_SAVE_CONFIG_VAR(occupiedThreshold, c, s); } void TVoxelMap_LikelihoodOptions::writeToStream( mrpt::serialization::CArchive& out) const { - out << decimation << occupiedThreshold; + out << decimate_up_to << occupiedThreshold; } void TVoxelMap_LikelihoodOptions::readFromStream( mrpt::serialization::CArchive& in) { - in >> decimation >> occupiedThreshold; + in >> decimate_up_to >> occupiedThreshold; } diff --git a/libs/maps/src/maps/CVoxelMapRGB.cpp b/libs/maps/src/maps/CVoxelMapRGB.cpp index 8614c32143..874fd4562f 100644 --- a/libs/maps/src/maps/CVoxelMapRGB.cpp +++ b/libs/maps/src/maps/CVoxelMapRGB.cpp @@ -250,6 +250,41 @@ double CVoxelMapRGB::internal_computeObservationLikelihood( const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const { - THROW_EXCEPTION("TODO"); - return 0; + // build aux 3D pointcloud: + mrpt::maps::CSimplePointsMap pts; + pts.insertObservation(obs, takenFrom); + + if (pts.empty()) return 0; + + double log_lik = .0; // cummulative log likelihoo + + auto lambdaPointLikelihood = [&](float x, float y, float z) { + double probOcc = 0; + const bool voxelExists = getPointOccupancy(x, y, z, probOcc); + if (!voxelExists) return; + log_lik += probOcc; + }; + + const auto& xs = pts.getPointsBufferRef_x(); + const auto& ys = pts.getPointsBufferRef_y(); + const auto& zs = pts.getPointsBufferRef_z(); + + if (pts.size() <= likelihoodOptions.decimate_up_to) + { + for (size_t i = 0; i < pts.size(); ++i) + lambdaPointLikelihood(xs[i], ys[i], zs[i]); + } + else + { + const double delta = + static_cast(pts.size()) / likelihoodOptions.decimate_up_to; + + for (size_t i = 0; i < likelihoodOptions.decimate_up_to; ++i) + { + const auto idx = static_cast(i * delta); + lambdaPointLikelihood(xs[idx], ys[idx], zs[idx]); + } + } + + return log_lik; } From cacb810c222496e5d967bbe76b956a81d2b8a1a8 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 1 Feb 2024 18:05:25 +0100 Subject: [PATCH 05/10] Fix pymrpt wrapper --- .../src/mrpt/maps/CVoxelMapOccupancyBase.cpp | 4 +- .../mrpt/maps/CVoxelMapOccupancyBase_1.cpp | 2 +- .../mrpt/maps/CVoxelMapOccupancyBase_2.cpp | 2 +- python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi | 74 ++++++++++++++++++- .../mrpt/pymrpt/mrpt/obs/__init__.pyi | 14 ++-- 5 files changed, 84 insertions(+), 12 deletions(-) diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp index 027957aace..c8fbb2f73c 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp @@ -114,7 +114,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase(std::function< pybind11::module &(std cl.def( pybind11::init( [](){ return new mrpt::maps::TVoxelMap_LikelihoodOptions(); }, [](){ return new PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions(); } ) ); cl.def( pybind11::init( [](PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions const &o){ return new PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions(o); } ) ); cl.def( pybind11::init( [](mrpt::maps::TVoxelMap_LikelihoodOptions const &o){ return new mrpt::maps::TVoxelMap_LikelihoodOptions(o); } ) ); - cl.def_readwrite("decimation", &mrpt::maps::TVoxelMap_LikelihoodOptions::decimation); + cl.def_readwrite("decimate_up_to", &mrpt::maps::TVoxelMap_LikelihoodOptions::decimate_up_to); cl.def_readwrite("occupiedThreshold", &mrpt::maps::TVoxelMap_LikelihoodOptions::occupiedThreshold); cl.def("loadFromConfigFile", (void (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(const class mrpt::config::CConfigFileBase &, const std::string &)) &mrpt::maps::TVoxelMap_LikelihoodOptions::loadFromConfigFile, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::loadFromConfigFile(const class mrpt::config::CConfigFileBase &, const std::string &) --> void", pybind11::arg("source"), pybind11::arg("section")); cl.def("saveToConfigFile", (void (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(class mrpt::config::CConfigFileBase &, const std::string &) const) &mrpt::maps::TVoxelMap_LikelihoodOptions::saveToConfigFile, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::saveToConfigFile(class mrpt::config::CConfigFileBase &, const std::string &) const --> void", pybind11::arg("c"), pybind11::arg("s")); @@ -122,7 +122,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase(std::function< pybind11::module &(std cl.def("readFromStream", (void (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(class mrpt::serialization::CArchive &)) &mrpt::maps::TVoxelMap_LikelihoodOptions::readFromStream, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::readFromStream(class mrpt::serialization::CArchive &) --> void", pybind11::arg("in")); cl.def("assign", (struct mrpt::maps::TVoxelMap_LikelihoodOptions & (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(const struct mrpt::maps::TVoxelMap_LikelihoodOptions &)) &mrpt::maps::TVoxelMap_LikelihoodOptions::operator=, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::operator=(const struct mrpt::maps::TVoxelMap_LikelihoodOptions &) --> struct mrpt::maps::TVoxelMap_LikelihoodOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::TVoxelMap_RenderingOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:77 + { // mrpt::maps::TVoxelMap_RenderingOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:78 pybind11::class_> cl(M("mrpt::maps"), "TVoxelMap_RenderingOptions", "Options for the conversion of a mrpt::maps::COctoMap into a\n mrpt::opengl::COctoMapVoxels "); cl.def( pybind11::init( [](){ return new mrpt::maps::TVoxelMap_RenderingOptions(); } ) ); cl.def( pybind11::init( [](mrpt::maps::TVoxelMap_RenderingOptions const &o){ return new mrpt::maps::TVoxelMap_RenderingOptions(o); } ) ); diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp index 6cd258ca20..00e84db3e8 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp @@ -50,7 +50,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:116 + { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:117 pybind11::class_, std::shared_ptr>, mrpt::maps::CVoxelMapBase, mrpt::maps::NearestNeighborsCapable> cl(M("mrpt::maps"), "CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t", ""); cl.def_readwrite("insertionOptions", &mrpt::maps::CVoxelMapOccupancyBase::insertionOptions); cl.def_readwrite("likelihoodOptions", &mrpt::maps::CVoxelMapOccupancyBase::likelihoodOptions); diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp index f76785b4a5..11afabccbc 100644 --- a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp @@ -50,7 +50,7 @@ void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:116 + { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:117 pybind11::class_, std::shared_ptr>, mrpt::maps::CVoxelMapBase, mrpt::maps::NearestNeighborsCapable> cl(M("mrpt::maps"), "CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t", ""); cl.def_readwrite("insertionOptions", &mrpt::maps::CVoxelMapOccupancyBase::insertionOptions); cl.def_readwrite("likelihoodOptions", &mrpt::maps::CVoxelMapOccupancyBase::likelihoodOptions); diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi index a4655225f5..069b91b0aa 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi @@ -488,6 +488,18 @@ class CColouredPointsMap(CPointsMap): def insertPointFast(self, x: float, y: float) -> None: ... @overload def insertPointFast(self, x: float, y: float, z: float) -> None: ... + @overload + def insertPointField_color_B(self, v: float) -> None: ... + @overload + def insertPointField_color_B(float) -> void: ... + @overload + def insertPointField_color_G(self, v: float) -> None: ... + @overload + def insertPointField_color_G(float) -> void: ... + @overload + def insertPointField_color_R(self, v: float) -> None: ... + @overload + def insertPointField_color_R(float) -> void: ... def insertPointRGB(self, x: float, y: float, z: float, R: float, G: float, B: float) -> None: ... def reserve(self, newLength: int) -> None: ... @overload @@ -2872,6 +2884,30 @@ class CPointsMap(CMetricMap, mrpt.pymrpt.mrpt.opengl.PLY_Importer, mrpt.pymrpt.m @overload def hasColorPoints() -> bool: ... @overload + def hasField_Intensity(self) -> bool: ... + @overload + def hasField_Intensity() -> bool: ... + @overload + def hasField_Ring(self) -> bool: ... + @overload + def hasField_Ring() -> bool: ... + @overload + def hasField_Timestamp(self) -> bool: ... + @overload + def hasField_Timestamp() -> bool: ... + @overload + def hasField_color_B(self) -> bool: ... + @overload + def hasField_color_B() -> bool: ... + @overload + def hasField_color_G(self) -> bool: ... + @overload + def hasField_color_G() -> bool: ... + @overload + def hasField_color_R(self) -> bool: ... + @overload + def hasField_color_R() -> bool: ... + @overload def insertAnotherMap(self, otherMap: CPointsMap, otherPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> None: ... @overload def insertAnotherMap(self, otherMap: CPointsMap, otherPose: mrpt.pymrpt.mrpt.poses.CPose3D, filterOutPointsAtZero: bool) -> None: ... @@ -2887,6 +2923,34 @@ class CPointsMap(CMetricMap, mrpt.pymrpt.mrpt.opengl.PLY_Importer, mrpt.pymrpt.m def insertPointFast(self, x: float, y: float) -> None: ... @overload def insertPointFast(self, x: float, y: float, z: float) -> None: ... + @overload + def insertPointField_Intensity(self, i: float) -> None: ... + @overload + def insertPointField_Intensity(float) -> void: ... + @overload + def insertPointField_Ring(self, r: int) -> None: ... + @overload + def insertPointField_Ring(uint16_t) -> void: ... + @overload + def insertPointField_Timestamp(self, t: float) -> None: ... + @overload + def insertPointField_Timestamp(float) -> void: ... + @overload + def insertPointField_color_B(self, v: float) -> None: ... + @overload + def insertPointField_color_B(float) -> void: ... + @overload + def insertPointField_color_G(self, v: float) -> None: ... + @overload + def insertPointField_color_G(float) -> void: ... + @overload + def insertPointField_color_R(self, v: float) -> None: ... + @overload + def insertPointField_color_R(float) -> void: ... + @overload + def insertPointFrom(self, source: CPointsMap, sourcePointIndex: int) -> None: ... + @overload + def insertPointFrom(constclassmrpt, size_t) -> void: ... def insertPointRGB(self, x: float, y: float, z: float, R: float, G: float, B: float) -> None: ... def internal_computeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation, takenFrom: mrpt.pymrpt.mrpt.poses.CPose3D) -> float: ... def internal_computeObservationLikelihoodPointCloud3D(self, pc_in_map: mrpt.pymrpt.mrpt.poses.CPose3D, xs: float, ys: float, zs: float, num_pts: int) -> float: ... @@ -3044,6 +3108,10 @@ class CPointsMapXYZI(CPointsMap): def insertPointFast(self, x: float, y: float) -> None: ... @overload def insertPointFast(self, x: float, y: float, z: float) -> None: ... + @overload + def insertPointField_Intensity(self, i: float) -> None: ... + @overload + def insertPointField_Intensity(float) -> void: ... def insertPointRGB(self, x: float, y: float, z: float, R_intensity: float, G_ignored: float, B_ignored: float) -> None: ... @overload def loadFromKittiVelodyneFile(self, filename: str) -> bool: ... @@ -3059,6 +3127,10 @@ class CPointsMapXYZI(CPointsMap): @overload def resize(size_t) -> void: ... @overload + def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... + @overload + def saveMetricMapRepresentationToFile(conststd) -> void: ... + @overload def saveToKittiVelodyneFile(self, filename: str) -> bool: ... @overload def saveToKittiVelodyneFile(conststd) -> bool: ... @@ -4329,7 +4401,7 @@ class TVoxelMap_InsertionOptions(mrpt.pymrpt.mrpt.config.CLoadableOptions): def writeToStream(classmrpt) -> void: ... class TVoxelMap_LikelihoodOptions(mrpt.pymrpt.mrpt.config.CLoadableOptions): - decimation: int + decimate_up_to: int occupiedThreshold: float @overload def __init__(self) -> None: ... diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi index 0529ea041f..5c5b29e757 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi @@ -615,7 +615,7 @@ class CObservation3DRangeScan(CObservation): def getZoneAsObs(self, obs: CObservation3DRangeScan, r1: int, r2: int, c1: int, c2: int) -> None: ... def get_unproj_lut(self, *args, **kwargs) -> Any: ... def hasPixelLabels(self) -> bool: ... - def load(self) -> None: ... + def load_impl(self) -> None: ... def points3D_convertToExternalStorage(self, fileName: str, use_this_base_dir: str) -> None: ... def points3D_getExternalStorageFile(self) -> str: ... @overload @@ -1050,9 +1050,9 @@ class CObservationImage(CObservation): @overload def getUndistortedImage(classmrpt) -> void: ... @overload - def load(self) -> None: ... + def load_impl(self) -> None: ... @overload - def load() -> void: ... + def load_impl() -> void: ... @overload def setSensorPose(self, newSensorPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> None: ... @overload @@ -1129,7 +1129,7 @@ class CObservationPointCloud(CObservation): @overload def getSensorPose(classmrpt) -> void: ... def isExternallyStored(self) -> bool: ... - def load(self) -> None: ... + def load_impl(self) -> None: ... @overload def overrideExternalStorageFormatFlag(self, fmt: CObservationPointCloud.ExternalStorageFormat) -> None: ... @overload @@ -1389,11 +1389,11 @@ class CObservationRotatingScan(CObservation): @overload def getSensorPose(classmrpt) -> void: ... def isExternallyStored(self) -> bool: ... - def load(self) -> None: ... @overload def loadFromTextFile(self, filename: str) -> bool: ... @overload def loadFromTextFile(conststd) -> bool: ... + def load_impl(self) -> None: ... @overload def overrideExternalStorageFormatFlag(self, fmt: CObservationRotatingScan.ExternalStorageFormat) -> None: ... @overload @@ -1485,9 +1485,9 @@ class CObservationStereoImages(CObservation): def getSensorPose(classmrpt) -> void: ... def getStereoCameraParams(self, *args, **kwargs) -> Any: ... @overload - def load(self) -> None: ... + def load_impl(self) -> None: ... @overload - def load() -> void: ... + def load_impl() -> void: ... @overload def setSensorPose(self, newSensorPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> None: ... @overload From 78231a79c4941505e8097b1d64816b63549e6c7b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sun, 4 Feb 2024 01:40:23 +0100 Subject: [PATCH 06/10] Add env var for debugging --- doc/source/doxygen-docs/changelog.md | 1 + doc/source/env-vars.rst | 2 ++ libs/maps/src/obs/CObservationPointCloud.cpp | 14 ++++++++++++++ 3 files changed, 17 insertions(+) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 9cd3e6600d..a5ef714b84 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -4,6 +4,7 @@ - Changes in libraries: - \ref mrpt_maps_grp: - Implement missing probabilistic observation models in mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB + - Add debug env variable `MRPT_DEBUG_OBSPTS_LAZY_LOAD` to debug lazy-load point cloud observations. - \ref mrpt_ros2bridge_grp: - Fix wrong macros leading to including obsolete header ``. diff --git a/doc/source/env-vars.rst b/doc/source/env-vars.rst index 3218b20198..8e3a1ac99d 100644 --- a/doc/source/env-vars.rst +++ b/doc/source/env-vars.rst @@ -24,6 +24,8 @@ numerical value (e.g. ``1``) will be interpreted as "true". - ``MRPT_DEBUG_OBSIMG_LAZY_LOAD``: Shows `std::cout` traces whenever an mrpt::obs::CObservationImage with delay-load content is loaded or unloaded. +- ``MRPT_DEBUG_OBSPTS_LAZY_LOAD``: Shows `std::cout` traces whenever an mrpt::obs::CObservationPointCloud with delay-load content is loaded or unloaded. + - ``MRPT_EXPR_VERBOSE``: Set to ``1`` to enable extra verbose debug traces for `mrpt::expr::CRuntimeCompiledExpression::eval() `_. diff --git a/libs/maps/src/obs/CObservationPointCloud.cpp b/libs/maps/src/obs/CObservationPointCloud.cpp index 46cc6f4652..d5749dda5a 100644 --- a/libs/maps/src/obs/CObservationPointCloud.cpp +++ b/libs/maps/src/obs/CObservationPointCloud.cpp @@ -9,6 +9,7 @@ #include "maps-precomp.h" // Precomp header // +#include #include #include #include @@ -111,6 +112,12 @@ void CObservationPointCloud::load_impl() const { MRPT_START + const thread_local bool MRPT_DEBUG_OBSPTS_LAZY_LOAD = + mrpt::get_env("MRPT_DEBUG_OBSPTS_LAZY_LOAD", false); + if (MRPT_DEBUG_OBSPTS_LAZY_LOAD) + std::cout << "[CObservationPointCloud::load()] Called on this=" + << reinterpret_cast(this) << std::endl; + // Already loaded? if (!isExternallyStored() || (isExternallyStored() && pointcloud)) return; @@ -201,6 +208,13 @@ void CObservationPointCloud::load_impl() const void CObservationPointCloud::unload() const { MRPT_START + + const thread_local bool MRPT_DEBUG_OBSPTS_LAZY_LOAD = + mrpt::get_env("MRPT_DEBUG_OBSPTS_LAZY_LOAD", false); + if (MRPT_DEBUG_OBSPTS_LAZY_LOAD) + std::cout << "[CObservationPointCloud::unload()] Called on this=" + << reinterpret_cast(this) << std::endl; + if (!isExternallyStored() || !pointcloud) return; // Free memory, saving to the file if it doesn't exist: From 6d09071d4430ab1c4b2f71034a0e11d23ffe278b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 5 Feb 2024 00:11:46 +0100 Subject: [PATCH 07/10] ROS1 rosbag2rawlog: added support for XYZIRT point clouds --- apps/rosbag2rawlog/rosbag2rawlog_main.cpp | 17 +++++++++++++++++ doc/source/doxygen-docs/changelog.md | 3 +++ 2 files changed, 20 insertions(+) diff --git a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp b/apps/rosbag2rawlog/rosbag2rawlog_main.cpp index f1b29e6061..9bbb3c907f 100644 --- a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp +++ b/apps/rosbag2rawlog/rosbag2rawlog_main.cpp @@ -211,6 +211,23 @@ Obs toPointCloud2(std::string_view msg, const rosbag::MessageInstance& rosmsg) if (!fields.count("x") || !fields.count("y") || !fields.count("z")) return {}; + if (fields.count("ring") || fields.count("time")) + { + // XYZIRT + auto mrptPts = mrpt::maps::CPointsMapXYZIRT::Create(); + ptsObs->pointcloud = mrptPts; + + if (!mrpt::ros2bridge::fromROS(pts, *mrptPts)) + { + THROW_EXCEPTION( + "Could not convert pointcloud from ROS to CPointsMapXYZIRT"); + } + else + { // converted ok: + return {ptsObs}; + } + } + if (fields.count("intensity")) { // XYZI diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index a5ef714b84..8427bc67c3 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,6 +1,9 @@ \page changelog Change Log # Version 2.11.8: UNRELEASED +- Changes in apps: + - rosbag2rawlog (ROS 1): + - Added support for XYZIRT point clouds. - Changes in libraries: - \ref mrpt_maps_grp: - Implement missing probabilistic observation models in mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB From 20a43c6c02028b3e0edcbc1d60483fa131a17dc2 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 5 Feb 2024 00:50:52 +0100 Subject: [PATCH 08/10] improvements to ros1 API --- apps/rosbag2rawlog/rosbag2rawlog_main.cpp | 338 +++++++++++------- doc/source/doxygen-docs/changelog.md | 3 + .../include/mrpt/ros1bridge/point_cloud2.h | 7 + libs/ros1bridge/src/point_cloud2.cpp | 62 ++++ 4 files changed, 287 insertions(+), 123 deletions(-) diff --git a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp b/apps/rosbag2rawlog/rosbag2rawlog_main.cpp index 9bbb3c907f..a6a65a96c3 100644 --- a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp +++ b/apps/rosbag2rawlog/rosbag2rawlog_main.cpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -12,7 +12,8 @@ // Intention: Parse bag files, save // as a RawLog file, easily readable by MRPT C++ programs. // -// Started: HL @ SEPT-2018 +// Started: Hunter Laux @ SEPT-2018. +// Maintained: JLBC @ 2018-2023 // =========================================================================== #include // this header is obsolete in ros2-I but as long as this app is only built for ros1 we are ok @@ -50,7 +51,9 @@ #include #include #include +#include #include +#include // needed by tf2::fromMsg() #include #include @@ -62,7 +65,7 @@ using namespace mrpt::system; using namespace std; // Declare the supported command line switches =========== -TCLAP::CmdLine cmd("rosbag2rawlog", ' ', MRPT_getVersion().c_str()); +TCLAP::CmdLine cmd("rosbag2rawlog (ROS 1)", ' ', MRPT_getVersion().c_str()); TCLAP::UnlabeledMultiArg arg_input_files( "bags", "Input bag files (required) (*.bag)", true, "log.bag", cmd); @@ -78,8 +81,10 @@ TCLAP::SwitchArg arg_overwrite( "w", "overwrite", "Force overwrite target file without prompting.", cmd, false); -TCLAP::ValueArg arg_world_frame( - "f", "frame", "World TF frame", true, "", "world", cmd); +TCLAP::ValueArg arg_base_link_frame( + "b", "base-link", + "Reference /tf frame for the robot frame (Default: 'base_link')", false, + "base_link", "base_link", cmd); using Obs = std::list; @@ -95,11 +100,8 @@ class RosSynchronizer using Callback = std::function&...)>; RosSynchronizer( - std::string_view rootFrame, std::shared_ptr tfBuffer, - const Callback& callback) - : m_rootFrame(rootFrame), - m_tfBuffer(std::move(tfBuffer)), - m_callback(callback) + std::shared_ptr tfBuffer, const Callback& callback) + : m_tfBuffer(std::move(tfBuffer)), m_callback(callback) { } @@ -111,42 +113,7 @@ class RosSynchronizer return ptr; } - Obs signal() - { - auto& frame = std::get<0>(m_cache)->header.frame_id; - auto& stamp = std::get<0>(m_cache)->header.stamp; - if (m_tfBuffer->canTransform(m_rootFrame, frame, stamp)) - { - auto t = m_tfBuffer->lookupTransform(m_rootFrame, frame, stamp); - mrpt::obs::CActionRobotMovement3D odom_move; - - auto& translate = t.transform.translation; - double x = translate.x; - double y = translate.y; - double z = translate.z; - - auto& q = t.transform.rotation; - mrpt::math::CQuaternion quat{q.w, q.x, q.y, q.z}; - - mrpt::poses::CPose3DQuat poseQuat(x, y, z, quat); - mrpt::poses::CPose3D currentPose(poseQuat); - - mrpt::poses::CPose3D incOdoPose(0, 0, 0, 0, 0, 0); - if (m_poseValid) { incOdoPose = currentPose - m_lastPose; } - m_lastPose = currentPose; - m_poseValid = true; - mrpt::obs::CActionRobotMovement3D::TMotionModelOptions model; - odom_move.computeFromOdometry(incOdoPose, model); - - auto acts = mrpt::obs::CActionCollection::Create(); - acts->insert(odom_move); - - auto obs = signal(std::make_index_sequence{}); - obs.push_front(acts); - return obs; - } - return {}; - } + Obs signal() { return {}; } template bool check(std::index_sequence) @@ -188,7 +155,6 @@ class RosSynchronizer } private: - std::string m_rootFrame; std::shared_ptr m_tfBuffer; Tuple m_cache; bool m_poseValid = false; @@ -196,7 +162,73 @@ class RosSynchronizer Callback m_callback; }; -Obs toPointCloud2(std::string_view msg, const rosbag::MessageInstance& rosmsg) +std::shared_ptr tfBuffer; + +std::set known_tf_frames; + +void removeTrailingSlash(std::string& s) +{ + ASSERT_(!s.empty()); + if (s.at(0) == '/') s = s.substr(1); +} + +void addTfFrameAsKnown(std::string s) +{ + removeTrailingSlash(s); + known_tf_frames.insert(s); +} + +bool findOutSensorPose( + mrpt::poses::CPose3D& des, std::string target_frame, + std::string source_frame, + const std::optional& fixedSensorPose) +{ + if (fixedSensorPose) + { + des = fixedSensorPose.value(); + return true; + } + + // TF1 old frames started with "/foo", forbidden in TF2. + // Handle this since this program is for importing old ROS1 bags: + removeTrailingSlash(target_frame); + removeTrailingSlash(source_frame); + + try + { + ASSERT_(tfBuffer); + + geometry_msgs::TransformStamped ref_to_trgFrame = + tfBuffer->lookupTransform( + target_frame, source_frame, {} /*latest value*/); + + tf2::Transform tf; + tf2::fromMsg(ref_to_trgFrame.transform, tf); + des = mrpt::ros1bridge::fromROS(tf); + +#if 0 + std::cout << mrpt::format( + "[findOutSensorPose] Found pose %s -> %s: %s\n", + source_frame.c_str(), target_frame.c_str(), des.asString().c_str()); +#endif + + return true; + } + catch (const tf2::TransformException& ex) + { + std::cerr << "findOutSensorPose: " << ex.what() << std::endl + << "\nKnown TF frames: "; + for (const auto& f : known_tf_frames) + std::cerr << "'" << f << "',"; + std::cerr << std::endl; + + return false; + } +} + +Obs toPointCloud2( + std::string_view msg, const rosbag::MessageInstance& rosmsg, + const std::optional& fixedSensorPose) { auto pts = rosmsg.instantiate(); @@ -204,6 +236,11 @@ Obs toPointCloud2(std::string_view msg, const rosbag::MessageInstance& rosmsg) ptsObs->sensorLabel = msg; ptsObs->timestamp = mrpt::ros1bridge::fromROS(pts->header.stamp); + bool sensorPoseOK = findOutSensorPose( + ptsObs->sensorPose, pts->header.frame_id, + arg_base_link_frame.getValue(), fixedSensorPose); + ASSERT_(sensorPoseOK); + // Convert points: std::set fields = mrpt::ros1bridge::extractFields(*pts); @@ -217,7 +254,7 @@ Obs toPointCloud2(std::string_view msg, const rosbag::MessageInstance& rosmsg) auto mrptPts = mrpt::maps::CPointsMapXYZIRT::Create(); ptsObs->pointcloud = mrptPts; - if (!mrpt::ros2bridge::fromROS(pts, *mrptPts)) + if (!mrpt::ros1bridge::fromROS(*pts, *mrptPts)) { THROW_EXCEPTION( "Could not convert pointcloud from ROS to CPointsMapXYZIRT"); @@ -241,9 +278,13 @@ Obs toPointCloud2(std::string_view msg, const rosbag::MessageInstance& rosmsg) { warn1st = true; std::cerr << "Could not convert pointcloud from ROS to " - "CPointsMapXYZI. Trying another format.\n"; + "CPointsMapXYZI. Trying with XYZ.\n"; } } + else + { // converted ok: + return {ptsObs}; + } } { @@ -259,23 +300,32 @@ Obs toPointCloud2(std::string_view msg, const rosbag::MessageInstance& rosmsg) return {ptsObs}; } -Obs toLidar2D(std::string_view msg, const rosbag::MessageInstance& rosmsg) +Obs toLidar2D( + std::string_view msg, const rosbag::MessageInstance& rosmsg, + const std::optional& fixedSensorPose) { auto scan = rosmsg.instantiate(); auto scanObs = mrpt::obs::CObservation2DRangeScan::Create(); - MRPT_TODO("Extract sensor pose from tf frames"); + // Extract sensor pose from tf frames, if enabled: mrpt::poses::CPose3D sensorPose; mrpt::ros1bridge::fromROS(*scan, sensorPose, *scanObs); scanObs->sensorLabel = msg; scanObs->timestamp = mrpt::ros1bridge::fromROS(scan->header.stamp); + bool sensorPoseOK = findOutSensorPose( + scanObs->sensorPose, scan->header.frame_id, + arg_base_link_frame.getValue(), fixedSensorPose); + ASSERT_(sensorPoseOK); + return {scanObs}; } -Obs toRotatingScan(std::string_view msg, const rosbag::MessageInstance& rosmsg) +Obs toRotatingScan( + std::string_view msg, const rosbag::MessageInstance& rosmsg, + const std::optional& fixedSensorPose) { auto pts = rosmsg.instantiate(); @@ -289,7 +339,6 @@ Obs toRotatingScan(std::string_view msg, const rosbag::MessageInstance& rosmsg) // As a structured 2D range images, if we have ring numbers: auto obsRotScan = mrpt::obs::CObservationRotatingScan::Create(); - MRPT_TODO("Extract sensor pose from tf frames"); const mrpt::poses::CPose3D sensorPose; if (!mrpt::ros1bridge::fromROS(*pts, *obsRotScan, sensorPose)) @@ -302,20 +351,32 @@ Obs toRotatingScan(std::string_view msg, const rosbag::MessageInstance& rosmsg) obsRotScan->sensorLabel = msg; obsRotScan->timestamp = mrpt::ros1bridge::fromROS(pts->header.stamp); + bool sensorPoseOK = findOutSensorPose( + obsRotScan->sensorPose, pts->header.frame_id, + arg_base_link_frame.getValue(), fixedSensorPose); + ASSERT_(sensorPoseOK); + return {obsRotScan}; } -Obs toIMU(std::string_view msg, const rosbag::MessageInstance& rosmsg) +Obs toIMU( + std::string_view msg, const rosbag::MessageInstance& rosmsg, + const std::optional& fixedSensorPose) { - auto pts = rosmsg.instantiate(); + auto imu = rosmsg.instantiate(); auto mrptObs = mrpt::obs::CObservationIMU::Create(); mrptObs->sensorLabel = msg; - mrptObs->timestamp = mrpt::ros1bridge::fromROS(pts->header.stamp); + mrptObs->timestamp = mrpt::ros1bridge::fromROS(imu->header.stamp); // Convert data: - mrpt::ros1bridge::fromROS(*pts, *mrptObs); + mrpt::ros1bridge::fromROS(*imu, *mrptObs); + + bool sensorPoseOK = findOutSensorPose( + mrptObs->sensorPose, imu->header.frame_id, + arg_base_link_frame.getValue(), fixedSensorPose); + ASSERT_(sensorPoseOK); return {mrptObs}; } @@ -341,9 +402,53 @@ Obs toOdometry(std::string_view msg, const rosbag::MessageInstance& rosmsg) return {mrptObs}; } +Obs toImage( + std::string_view msg, const rosbag::MessageInstance& rosmsg, + const std::optional& fixedSensorPose) +{ + cv_bridge::CvImagePtr cv_ptr; + mrpt::Clock::time_point timeStamp; + std::string frame_id; + + if (auto image = rosmsg.instantiate(); image) + { + cv_ptr = cv_bridge::toCvCopy(image); + timeStamp = mrpt::ros1bridge::fromROS(image->header.stamp); + frame_id = image->header.frame_id; + } + else if (auto imC = rosmsg.instantiate(); imC) + { + cv_ptr = cv_bridge::toCvCopy(imC); + timeStamp = mrpt::ros1bridge::fromROS(imC->header.stamp); + frame_id = imC->header.frame_id; + } + else + { + THROW_EXCEPTION_FMT( + "Unhandled ROS topic '%s' type: images are expected to be either " + "sensor_msgs::Image or sensor_msgs::CompressedImage", + std::string(msg).c_str()); + } + + auto imgObs = mrpt::obs::CObservationImage::Create(); + + imgObs->sensorLabel = msg; + imgObs->timestamp = timeStamp; + + imgObs->image = mrpt::img::CImage(cv_ptr->image, mrpt::img::SHALLOW_COPY); + + bool sensorPoseOK = findOutSensorPose( + imgObs->cameraPose, frame_id, arg_base_link_frame.getValue(), + fixedSensorPose); + ASSERT_(sensorPoseOK); + + return {imgObs}; +} + Obs toRangeImage( std::string_view msg, const sensor_msgs::Image::Ptr& image, - const sensor_msgs::CameraInfo::Ptr& cameraInfo, bool rangeIsDepth) + const sensor_msgs::CameraInfo::Ptr& cameraInfo, bool rangeIsDepth, + const std::optional& fixedSensorPose) { auto cv_ptr = cv_bridge::toCvShare(image); @@ -352,15 +457,6 @@ Obs toRangeImage( { auto rangeScan = mrpt::obs::CObservation3DRangeScan::Create(); - // MRPT assumes the image plane is parallel to the YZ plane, so the - // camera is pointed in the X direction ROS assumes the image plane - // is parallel to XY plane, so the camera is pointed in the Z - // direction Apply a rotation to convert between these conventions. - mrpt::math::CQuaternion rot{0.5, 0.5, -0.5, 0.5}; - mrpt::poses::CPose3DQuat poseQuat(0, 0, 0, rot); - mrpt::poses::CPose3D pose(poseQuat); - rangeScan->setSensorPose(pose); - rangeScan->sensorLabel = msg; rangeScan->timestamp = mrpt::ros1bridge::fromROS(image->header.stamp); @@ -390,46 +486,29 @@ Obs toRangeImage( rangeScan->range_is_depth = rangeIsDepth; - return {rangeScan}; - } - return {}; -} + bool sensorPoseOK = findOutSensorPose( + rangeScan->sensorPose, image->header.frame_id, + arg_base_link_frame.getValue(), fixedSensorPose); + ASSERT_(sensorPoseOK); -Obs toImage(std::string_view msg, const rosbag::MessageInstance& rosmsg) -{ - cv_bridge::CvImagePtr cv_ptr; - mrpt::Clock::time_point timeStamp; +#if 0 // JLBC: not needed anymore? + // MRPT assumes the image plane is parallel to the YZ plane, so the + // camera is pointed in the X direction ROS assumes the image plane + // is parallel to XY plane, so the camera is pointed in the Z + // direction Apply a rotation to convert between these conventions. + mrpt::math::CQuaternion rot{0.5, 0.5, -0.5, 0.5}; + mrpt::poses::CPose3DQuat poseQuat(0, 0, 0, rot); + mrpt::poses::CPose3D pose(poseQuat); + rangeScan->setSensorPose(pose); +#endif - if (auto image = rosmsg.instantiate(); image) - { - cv_ptr = cv_bridge::toCvCopy(image); - timeStamp = mrpt::ros1bridge::fromROS(image->header.stamp); - } - else if (auto imC = rosmsg.instantiate(); imC) - { - cv_ptr = cv_bridge::toCvCopy(imC); - timeStamp = mrpt::ros1bridge::fromROS(imC->header.stamp); - } - else - { - THROW_EXCEPTION_FMT( - "Unhandled ROS topic '%s' type: images are expected to be either " - "sensor_msgs::Image or sensor_msgs::CompressedImage", - std::string(msg).c_str()); + return {rangeScan}; } - - auto imgObs = mrpt::obs::CObservationImage::Create(); - - imgObs->sensorLabel = msg; - imgObs->timestamp = timeStamp; - - imgObs->image = mrpt::img::CImage(cv_ptr->image, mrpt::img::SHALLOW_COPY); - - return {imgObs}; + return {}; } template -Obs toTf(tf2::BufferCore& tfBuffer, const rosbag::MessageInstance& rosmsg) +Obs toTf(tf2::BufferCore& tfBuf, const rosbag::MessageInstance& rosmsg) { if (rosmsg.getDataType() == "tf2_msgs/TFMessage") { @@ -438,7 +517,14 @@ Obs toTf(tf2::BufferCore& tfBuffer, const rosbag::MessageInstance& rosmsg) { try { - tfBuffer.setTransform(tf, "bagfile", isStatic); + tfBuf.setTransform(tf, "bagfile", isStatic); + + addTfFrameAsKnown(tf.child_frame_id); + addTfFrameAsKnown(tf.header.frame_id); +#if 0 + std::cout << "tf: " << tf.child_frame_id + << " <= " << tf.header.frame_id << "\n"; +#endif } catch (const tf2::TransformException& ex) { @@ -452,11 +538,9 @@ Obs toTf(tf2::BufferCore& tfBuffer, const rosbag::MessageInstance& rosmsg) class Transcriber { public: - Transcriber( - std::string_view rootFrame, const mrpt::containers::yaml& config) - : m_rootFrame(rootFrame) + Transcriber(const mrpt::containers::yaml& config) { - auto tfBuffer = std::make_shared(); + tfBuffer = std::make_shared(); m_lookup["/tf"].emplace_back( [=](const rosbag::MessageInstance& rosmsg) { @@ -473,6 +557,16 @@ class Transcriber auto& sensor = sensorNode.second.asMap(); const auto sensorType = sensor.at("type").as(); + // Optional: fixed sensorPose (then ignores/don't need "tf" data): + std::optional fixedSensorPose; + if (sensor.count("fixed_sensor_pose") != 0) + { + fixedSensorPose = mrpt::poses::CPose3D::FromString( + "["s + sensor.at("fixed_sensor_pose").as() + + "]"s); + } + +#if 0 if (sensorType == "CObservation3DRangeScan") { bool rangeIsDepth = sensor.count("rangeIsDepth") @@ -480,22 +574,24 @@ class Transcriber : true; auto callback = [=](const sensor_msgs::Image::Ptr& image, const sensor_msgs::CameraInfo::Ptr& info) { - return toRangeImage(sensorName, image, info, rangeIsDepth); + return toRangeImage( + sensorName, image, info, rangeIsDepth, fixedSensorPose); }; using Synchronizer = RosSynchronizer< sensor_msgs::Image, sensor_msgs::CameraInfo>; - auto sync = std::make_shared( - rootFrame, tfBuffer, callback); + auto sync = std::make_shared(tfBuffer, callback); m_lookup[sensor.at("depth").as()].emplace_back( sync->bind<0>()); m_lookup[sensor.at("cameraInfo").as()] .emplace_back(sync->bind<1>()); m_lookup["/tf"].emplace_back(sync->bindTfSync()); } - else if (sensorType == "CObservationImage") + else +#endif + if (sensorType == "CObservationImage") { auto callback = [=](const rosbag::MessageInstance& m) { - return toImage(sensorName, m); + return toImage(sensorName, m, fixedSensorPose); }; ASSERT_(sensor.count("image_topic") != 0); m_lookup[sensor.at("image_topic").as()] @@ -504,25 +600,23 @@ class Transcriber else if (sensorType == "CObservationPointCloud") { auto callback = [=](const rosbag::MessageInstance& m) { - return toPointCloud2(sensorName, m); + return toPointCloud2(sensorName, m, fixedSensorPose); }; m_lookup[sensor.at("topic").as()].emplace_back( callback); - // m_lookup["/tf"].emplace_back(sync->bindTfSync()); } else if (sensorType == "CObservation2DRangeScan") { auto callback = [=](const rosbag::MessageInstance& m) { - return toLidar2D(sensorName, m); + return toLidar2D(sensorName, m, fixedSensorPose); }; m_lookup[sensor.at("topic").as()].emplace_back( callback); - // m_lookup["/tf"].emplace_back(sync->bindTfSync()); } else if (sensorType == "CObservationRotatingScan") { auto callback = [=](const rosbag::MessageInstance& m) { - return toRotatingScan(sensorName, m); + return toRotatingScan(sensorName, m, fixedSensorPose); }; m_lookup[sensor.at("topic").as()].emplace_back( callback); @@ -530,11 +624,10 @@ class Transcriber else if (sensorType == "CObservationIMU") { auto callback = [=](const rosbag::MessageInstance& m) { - return toIMU(sensorName, m); + return toIMU(sensorName, m, fixedSensorPose); }; m_lookup[sensor.at("topic").as()].emplace_back( callback); - // m_lookup["/tf"].emplace_back(sync->bindTfSync()); } else if (sensorType == "CObservationOdometry") { @@ -574,7 +667,6 @@ class Transcriber }; private: - std::string m_rootFrame; std::map> m_lookup; std::set m_unhandledTopics; }; @@ -626,7 +718,7 @@ int main(int argc, char** argv) throw std::runtime_error("Error writing file!"); auto arch = archiveFrom(fil_out); - Transcriber t(arg_world_frame.getValue(), config); + Transcriber t(config); const auto nEntries = full_view.size(); size_t curEntry = 0, showProgressCnt = 0; for (const auto& m : full_view) @@ -652,6 +744,7 @@ int main(int argc, char** argv) showProgressCnt = 0; } } + printf("\n"); for (auto& bag : bags) @@ -664,8 +757,7 @@ int main(int argc, char** argv) } catch (std::exception& e) { - if (strlen(e.what())) - std::cerr << mrpt::exception_to_str(e) << std::endl; + if (strlen(e.what())) std::cerr << e.what() << std::endl; return 1; } } // end of main() diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 8427bc67c3..3d79e896bb 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -4,10 +4,13 @@ - Changes in apps: - rosbag2rawlog (ROS 1): - Added support for XYZIRT point clouds. + - Finally, implemented automatic detection of sensor poses wrt `base_link` from `tf` messages, with an option to manually override sensor poses from YAML config. - Changes in libraries: - \ref mrpt_maps_grp: - Implement missing probabilistic observation models in mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB - Add debug env variable `MRPT_DEBUG_OBSPTS_LAZY_LOAD` to debug lazy-load point cloud observations. + - \ref mrpt_ros1bridge_grp: + - Add missing mrpt::ros1bridge::fromROS() for `PointCloud2` => mrpt::maps::CPointsMapXYZIRT conversions. - \ref mrpt_ros2bridge_grp: - Fix wrong macros leading to including obsolete header ``. diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud2.h b/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud2.h index a6583d3715..d17189ae9a 100644 --- a/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud2.h +++ b/libs/ros1bridge/include/mrpt/ros1bridge/point_cloud2.h @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -42,6 +43,12 @@ bool fromROS( bool fromROS( const sensor_msgs::PointCloud2& msg, mrpt::maps::CPointsMapXYZI& obj); +/** \overload For (x,y,z,intensity,ring,time) channels. + * Requires point cloud fields: x,y,z,intensity,ring,time + */ +bool fromROS( + const sensor_msgs::PointCloud2& msg, mrpt::maps::CPointsMapXYZIRT& obj); + /** Convert sensor_msgs/PointCloud2 -> mrpt::obs::CObservationRotatingScan. * Requires point cloud fields: x,y,z,intensity,ring */ diff --git a/libs/ros1bridge/src/point_cloud2.cpp b/libs/ros1bridge/src/point_cloud2.cpp index 406b17bc94..386933b3bb 100644 --- a/libs/ros1bridge/src/point_cloud2.cpp +++ b/libs/ros1bridge/src/point_cloud2.cpp @@ -168,6 +168,68 @@ bool mrpt::ros1bridge::fromROS( return true; } +bool mrpt::ros1bridge::fromROS( + const sensor_msgs::PointCloud2& msg, CPointsMapXYZIRT& obj) +{ + // Copy point data + unsigned int num_points = msg.width * msg.height; + + bool incompatible = false; + const sensor_msgs::PointField *x_field = nullptr, *y_field = nullptr, + *z_field = nullptr, *i_field = nullptr, + *r_field = nullptr, *t_field = nullptr; + + for (unsigned int i = 0; i < msg.fields.size() && !incompatible; i++) + { + incompatible |= check_field(msg.fields[i], "x", &x_field); + incompatible |= check_field(msg.fields[i], "y", &y_field); + incompatible |= check_field(msg.fields[i], "z", &z_field); + incompatible |= check_field(msg.fields[i], "intensity", &i_field); + incompatible |= check_field(msg.fields[i], "ring", &r_field); + incompatible |= check_field(msg.fields[i], "time", &t_field); + } + + if (incompatible || (!x_field || !y_field || !z_field)) return false; + + obj.resize_XYZIRT(num_points, !!i_field, !!r_field, !!t_field); + + unsigned int idx = 0; + for (unsigned int row = 0; row < msg.height; ++row) + { + const unsigned char* row_data = &msg.data[row * msg.row_step]; + for (uint32_t col = 0; col < msg.width; ++col, ++idx) + { + const unsigned char* msg_data = row_data + col * msg.point_step; + + float x, y, z; + get_float_from_field(x_field, msg_data, x); + get_float_from_field(y_field, msg_data, y); + get_float_from_field(z_field, msg_data, z); + obj.setPointFast(idx, x, y, z); + + if (i_field) + { + float i; + get_float_from_field(i_field, msg_data, i); + obj.setPointIntensity(idx, i); + } + if (r_field) + { + uint16_t ring_id = 0; + get_uint16_from_field(r_field, msg_data, ring_id); + obj.setPointRing(idx, ring_id); + } + if (t_field) + { + float t; + get_float_from_field(t_field, msg_data, t); + obj.setPointTime(idx, t); + } + } + } + return true; +} + bool mrpt::ros1bridge::toROS( const CSimplePointsMap& obj, const std_msgs::Header& msg_header, sensor_msgs::PointCloud2& msg) From 4c9da0fb51e50148d28c46964bd698688c727f47 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 5 Feb 2024 12:15:56 +0100 Subject: [PATCH 09/10] more rawlog info --- doc/source/doxygen-docs/changelog.md | 2 ++ libs/apps/src/rawlog-edit_info.cpp | 5 ++++ libs/maps/src/obs/CObservationPointCloud.cpp | 26 ++++++++++++++++++++ libs/math/include/mrpt/math/ops_containers.h | 5 ++-- 4 files changed, 36 insertions(+), 2 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 3d79e896bb..4939a33191 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -2,6 +2,8 @@ # Version 2.11.8: UNRELEASED - Changes in apps: + - RawLogViewer: Show pointcloud ring, intensity, and time min/max ranges. + - rawlog-edit: Show dataset duration as formatted time interval. - rosbag2rawlog (ROS 1): - Added support for XYZIRT point clouds. - Finally, implemented automatic detection of sensor poses wrt `base_link` from `tf` messages, with an option to manually override sensor poses from YAML config. diff --git a/libs/apps/src/rawlog-edit_info.cpp b/libs/apps/src/rawlog-edit_info.cpp index 5ffc8b0599..c75ae327a7 100644 --- a/libs/apps/src/rawlog-edit_info.cpp +++ b/libs/apps/src/rawlog-edit_info.cpp @@ -149,6 +149,11 @@ DECLARE_OP_FUNCTION(op_info) mrpt::Clock::fromDouble(proc.lastTimestamp)) << " UTC)\n"; + cout << "Duration : " + << mrpt::system::intervalFormat( + proc.lastTimestamp - proc.firstTimestamp) + << "\n"; + // By sensor labels: cout << "All sensor labels : "; for (auto it = proc.infoPerSensorLabel.begin(); diff --git a/libs/maps/src/obs/CObservationPointCloud.cpp b/libs/maps/src/obs/CObservationPointCloud.cpp index d5749dda5a..017f2041cb 100644 --- a/libs/maps/src/obs/CObservationPointCloud.cpp +++ b/libs/maps/src/obs/CObservationPointCloud.cpp @@ -17,6 +17,7 @@ #include #include #include +#include // minimum_maximum() #include #include #include @@ -58,6 +59,31 @@ void CObservationPointCloud::getDescriptionAsText(std::ostream& o) const { o << pointcloud->GetRuntimeClass()->className << "\n"; o << "Number of points: " << pointcloud->size() << "\n"; + + // Special cases: show IRT stats: + if (auto* ptrIs = pointcloud->getPointsBufferRef_intensity(); + ptrIs && !ptrIs->empty()) + { + float Imin, Imax; + mrpt::math::minimum_maximum(*ptrIs, Imin, Imax); + o << "Intensity channel values: min=" << Imin << " max=" << Imax + << "\n"; + } + if (auto* ptrTs = pointcloud->getPointsBufferRef_timestamp(); + ptrTs && !ptrTs->empty()) + { + float Tmin, Tmax; + mrpt::math::minimum_maximum(*ptrTs, Tmin, Tmax); + o << "Timestamp channel values: min=" << Tmin << " max=" << Tmax + << "\n"; + } + if (auto* ptrRs = pointcloud->getPointsBufferRef_ring(); + ptrRs && !ptrRs->empty()) + { + uint16_t Rmin, Rmax; + mrpt::math::minimum_maximum(*ptrRs, Rmin, Rmax); + o << "Ring channel values: min=" << Rmin << " max=" << Rmax << "\n"; + } } if (m_externally_stored != ExternalStorageFormat::None) diff --git a/libs/math/include/mrpt/math/ops_containers.h b/libs/math/include/mrpt/math/ops_containers.h index 02b2d0a945..17b23ed893 100644 --- a/libs/math/include/mrpt/math/ops_containers.h +++ b/libs/math/include/mrpt/math/ops_containers.h @@ -255,8 +255,9 @@ inline double mean(const CONTAINER& v) } /** Return the maximum and minimum values of a std::vector */ -template -inline void minimum_maximum(const std::vector& V, T& curMin, T& curMax) +template +inline void minimum_maximum( + const std::vector& V, T& curMin, T& curMax) { ASSERT_(V.size() != 0); const size_t N = V.size(); From 13efca58f192293bcaeed64fe599022d22b4267a Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Tue, 6 Feb 2024 23:46:06 +0100 Subject: [PATCH 10/10] Add hyperlink() --- doc/source/doxygen-docs/changelog.md | 2 + libs/system/include/mrpt/system/hyperlink.h | 28 ++++++++++++++ libs/system/src/hyperlink.cpp | 42 +++++++++++++++++++++ 3 files changed, 72 insertions(+) create mode 100644 libs/system/include/mrpt/system/hyperlink.h create mode 100644 libs/system/src/hyperlink.cpp diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 4939a33191..ba554ca4d2 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -15,6 +15,8 @@ - Add missing mrpt::ros1bridge::fromROS() for `PointCloud2` => mrpt::maps::CPointsMapXYZIRT conversions. - \ref mrpt_ros2bridge_grp: - Fix wrong macros leading to including obsolete header ``. + - \ref mrpt_system_grp: + - New function mrpt::system::hyperlink() to generate clickable links in terminal messages. # Version 2.11.7: Released Jan 25th, 2024 - Changes in apps: diff --git a/libs/system/include/mrpt/system/hyperlink.h b/libs/system/include/mrpt/system/hyperlink.h new file mode 100644 index 0000000000..74fd860b8e --- /dev/null +++ b/libs/system/include/mrpt/system/hyperlink.h @@ -0,0 +1,28 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ +#pragma once + +#include +#include + +namespace mrpt::system +{ +/** Build an text with a hyperlink, if printed to a terminal that supports it + * (e.g. Ubuntu >=21.04), or the plain text otherwise. + * + * \ingroup mrpt_system_grp + * \note (New in MRPT 2.11.8) + * \sa See discussion in, for example, + * https://gist.github.com/egmontkob/eb114294efbcd5adb1944c9f3cb5feda + */ +std::string hyperlink( + const std::string& text, const std::string& uri, + bool force_use_format = false, bool show_uri_anyway = false); + +} // namespace mrpt::system diff --git a/libs/system/src/hyperlink.cpp b/libs/system/src/hyperlink.cpp new file mode 100644 index 0000000000..db4bde81e9 --- /dev/null +++ b/libs/system/src/hyperlink.cpp @@ -0,0 +1,42 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "system-precomp.h" // Precompiled headers +// +#include +#include + +#ifndef _WIN32 +#include + +#include +#endif + +std::string mrpt::system::hyperlink( + const std::string& text, const std::string& uri, bool force_use_format, + bool show_uri_anyway) +{ + using namespace std::string_literals; + + bool supportsLinks = false; + +#ifndef _WIN32 + thread_local bool coutIsAtty = isatty(fileno(stdout)); + supportsLinks = coutIsAtty; +#endif + // See: https://gist.github.com/egmontkob/eb114294efbcd5adb1944c9f3cb5feda + if (supportsLinks || force_use_format) + return mrpt::format( + "\e]8;;%s\e\\%s\e]8;;\e\\", uri.c_str(), text.c_str()); + else + return show_uri_anyway ? // + (text + " ("s + uri + ")"s) + : // + text; +}