Merge branch 'develop'

Author: jlblancoc

apps/rosbag2rawlog/rosbag2rawlog_main.cpp

@@ -30,6 +30,7 @@
 #include <mrpt/obs/CObservationPointCloud.h>
 #include <mrpt/obs/CObservationRotatingScan.h>
 #include <mrpt/poses/CPose3DQuat.h>
+#include <mrpt/ros1bridge/gps.h>
 #include <mrpt/ros1bridge/imu.h>
 #include <mrpt/ros1bridge/laser_scan.h>
 #include <mrpt/ros1bridge/point_cloud2.h>
@@ -398,6 +399,33 @@ Obs toIMU(
   return {mrptObs};
 }
 
+Obs toGPS(
+    std::string_view msg,
+    const rosbag::MessageInstance& rosmsg,
+    const std::optional<mrpt::poses::CPose3D>& fixedSensorPose)
+{
+  auto gps = rosmsg.instantiate<sensor_msgs::NavSatFix>();
+
+  auto mrptObs = mrpt::obs::CObservationGPS::Create();
+
+  mrptObs->sensorLabel = msg;
+  mrptObs->timestamp = mrpt::ros1bridge::fromROS(gps->header.stamp);
+
+  // Convert data:
+  mrpt::ros1bridge::fromROS(*gps, *mrptObs);
+
+  bool sensorPoseOK = findOutSensorPose(
+      mrptObs->sensorPose, gps->header.frame_id, arg_base_link_frame.getValue(), fixedSensorPose);
+  if (!sensorPoseOK)
+  {
+    std::cerr << "Warning: dropping one observation of type '" << msg
+              << "' due to missing /tf data.\n";
+    return {};
+  }
+
+  return {mrptObs};
+}
+
 Obs toOdometry(std::string_view msg, const rosbag::MessageInstance& rosmsg)
 {
   auto odo = rosmsg.instantiate<nav_msgs::Odometry>();
@@ -676,6 +704,12 @@ class Transcriber
         { return toIMU(sensorName, m, fixedSensorPose); };
         m_lookup[sensor.at("topic").as<std::string>()].emplace_back(callback);
       }
+      else if (sensorType == "CObservationGPS")
+      {
+        auto callback = [=](const rosbag::MessageInstance& m)
+        { return toGPS(sensorName, m, fixedSensorPose); };
+        m_lookup[sensor.at("topic").as<std::string>()].emplace_back(callback);
+      }
       else if (sensorType == "CObservationOdometry")
       {
         auto callback = [=](const rosbag::MessageInstance& m) { return toOdometry(sensorName, m); };

appveyor.yml

@@ -1,5 +1,5 @@
 # version format
-version: 2.14.6-{branch}-build{build}
+version: 2.14.7-{branch}-build{build}
 
 os: Visual Studio 2019
 

doc/source/doxygen-docs/changelog.md

@@ -1,5 +1,16 @@
 \page changelog Change Log
 
+# Version 2.14.7: UNRELEASED
+- Changes in apps:
+  - rosbag2rawlog (ROS1): Implement conversion of NavSatFix -> mrpt::obs::CObservationGPS
+- Changes in libraries:
+  - \ref mrpt_math_grp
+    - mrpt::math::TBoundingBox: Renamed flag to bounding box from None to NoFlags to prevent autogenerated stubs from conflicting with python None type
+  - \ref mrpt_opengl_grp
+    - mrpt::opengl::Texture now caches "texture names" (OpenGL texture IDs) via image data, boosting performance of MVSim boot up time.
+- Build system:
+  - `mrpt-*-config.cmake` files now enforce the search of cmake dependencies in CONFIG mode, to avoid being foolish by deprecated `FindXXX()` lying around.
+
 # Version 2.14.6: Released Dec 3rd, 2024
 - Changes in libraries:
   - \ref mrpt_gui_grp:

libs/math/include/mrpt/math/TBoundingBox.h

@@ -28,7 +28,7 @@ struct TBoundingBox_
 {
   enum class CTOR_FLAGS
   {
-    None = 0,
+    NoFlags = 0,
     AllowUnordered
   };
 
@@ -43,7 +43,7 @@ struct TBoundingBox_
   TBoundingBox_(
       const mrpt::math::TPoint3D_<T>& Min,
       const mrpt::math::TPoint3D_<T>& Max,
-      const CTOR_FLAGS f = CTOR_FLAGS::None) :
+      const CTOR_FLAGS f = CTOR_FLAGS::NoFlags) :
       min(Min), max(Max)
   {
     if (f != CTOR_FLAGS::AllowUnordered)

libs/opengl/include/mrpt/opengl/Texture.h

@@ -134,7 +134,7 @@ class Texture
 
 // Normally users should not need to call these, but they are exposed just in
 // case they are useful someday.
-texture_name_t getNewTextureNumber();
+texture_name_t getNewTextureNumber(const uint8_t* optionalRgbDataForAssociation);
 void releaseTextureName(const texture_name_t& t);
 
 }  // namespace mrpt::opengl

libs/opengl/src/CRenderizableShaderTexturedTriangles.cpp

@@ -211,7 +211,7 @@ void CRenderizableShaderTexturedTriangles::assignImage(const CImage& img)
 
   m_glTexture.unloadTexture();
 
-  // Make a copy:
+  // Make a shallow copy:
   m_textureImage = img;
   m_textureImageAssigned = true;
 

libs/opengl/src/Texture.cpp

@@ -9,6 +9,7 @@
 
 #include "opengl-precomp.h"  // Precompiled header
 //
+#include <mrpt/containers/bimap.h>
 #include <mrpt/core/get_env.h>
 #include <mrpt/core/lock_helper.h>
 #include <mrpt/opengl/Texture.h>
@@ -57,8 +58,8 @@ class TextureResourceHandler
     return o;
   }
 
-  /// Return [textureName, textureUnit]
-  texture_name_t generateTextureID()
+  /// Return textureName
+  texture_name_t generateTextureID(const uint8_t* rgbDataForAssociation)
   {
 #if MRPT_HAS_OPENGL_GLUT || MRPT_HAS_EGL
     auto lck = mrpt::lockHelper(m_texturesMtx);
@@ -71,6 +72,8 @@ class TextureResourceHandler
     CHECK_OPENGL_ERROR_IN_DEBUG();
     m_textureReservedFrom[textureID] = std::this_thread::get_id();
 
+    if (rgbDataForAssociation) m_textureToRGBdata.insert(textureID, rgbDataForAssociation);
+
     if (MRPT_OPENGL_VERBOSE)
       std::cout << "[mrpt generateTextureID] textureName:" << textureID << std::endl;
 
@@ -80,6 +83,21 @@ class TextureResourceHandler
 #endif
   }
 
+  std::optional<texture_name_t> checkIfTextureAlreadyExists(const mrpt::img::CImage& rgb)
+  {
+#if MRPT_HAS_OPENGL_GLUT || MRPT_HAS_EGL
+    auto lck = mrpt::lockHelper(m_texturesMtx);
+
+    auto it = m_textureToRGBdata.getInverseMap().find(rgb.asCvMatRef().data);
+    if (it != m_textureToRGBdata.getInverseMap().end())
+      return it->second;
+    else
+      return {};
+#else
+    return {};
+#endif
+  }
+
   void releaseTextureID(unsigned int texName)
   {
 #if MRPT_HAS_OPENGL_GLUT || MRPT_HAS_EGL
@@ -109,19 +127,36 @@ class TextureResourceHandler
   void processDestroyQueue()
   {
 #if MRPT_HAS_OPENGL_GLUT || MRPT_HAS_EGL
-    if (auto itLst = m_destroyQueue.find(std::this_thread::get_id()); itLst != m_destroyQueue.end())
+    if (auto itLst = m_destroyQueue.find(std::this_thread::get_id());
+        itLst != m_destroyQueue.end() && !itLst->second.empty())
     {
       auto& lst = itLst->second;
+
+      // Delete in OpenGL:
       glDeleteTextures(lst.size(), lst.data());
       CHECK_OPENGL_ERROR_IN_DEBUG();
+
+      // delete in rgb data container too:
+      for (const auto id : lst)
+      {
+        if (m_textureToRGBdata.hasKey(id)) m_textureToRGBdata.erase_by_key(id);
+      }
+
+      if (MRPT_OPENGL_VERBOSE)
+      {
+        std::cout << "[mrpt processDestroyQueue] threadId=" << std::this_thread::get_id()
+                  << " destroyed " << lst.size() << "\n";
+      }
       lst.clear();
+      m_destroyQueue.erase(itLst);
     }
-    if (MRPT_OPENGL_VERBOSE)
+    if (!m_destroyQueue.empty() && MRPT_OPENGL_VERBOSE)
     {
       std::cout << "[mrpt processDestroyQueue] threadId=" << std::this_thread::get_id()
-                << ". At output: ";
+                << ". Remaining at output: ";
       for (const auto& lst : m_destroyQueue)
-        std::cout << "[" << lst.first << "]=" << lst.second.size() << " ";
+        std::cout << "[" << lst.first << "]=" << lst.second.size() << " textures ";
+      std::cout << "\n";
     }
 #endif
   }
@@ -130,14 +165,20 @@ class TextureResourceHandler
   std::mutex m_texturesMtx;
   std::map<GLuint, std::thread::id> m_textureReservedFrom;
   std::map<std::thread::id, std::vector<GLuint>> m_destroyQueue;
+  mrpt::containers::bimap<GLuint, const uint8_t*> m_textureToRGBdata;
   GLint m_maxTextureUnits;
 #endif
 };
 
+std::optional<texture_name_t> checkIfTextureAlreadyExists(const mrpt::img::CImage& rgb)
+{
+  return TextureResourceHandler::Instance().checkIfTextureAlreadyExists(rgb);
+}
+
 /// Returns: [texture name, texture unit]
-texture_name_t mrpt::opengl::getNewTextureNumber()
+texture_name_t mrpt::opengl::getNewTextureNumber(const uint8_t* optionalRgbDataForAssociation)
 {
-  return TextureResourceHandler::Instance().generateTextureID();
+  return TextureResourceHandler::Instance().generateTextureID(optionalRgbDataForAssociation);
 }
 
 void mrpt::opengl::releaseTextureName(const texture_name_t& t)
@@ -247,6 +288,21 @@ void Texture::internalAssignImage_2D(
   in_rgb->forceLoad();  // just in case they are lazy-load imgs
   if (in_alpha) in_alpha->forceLoad();
 
+  // Check if we already have this texture loaded in GPU and avoid creating
+  // duplicated texture ID:
+  const auto existingTextureId = checkIfTextureAlreadyExists(*in_rgb);
+  if (existingTextureId.has_value())
+  {
+    get() = existingTextureId.value();
+    get()->unit = textureUnit;
+
+    if (MRPT_OPENGL_VERBOSE)
+      std::cout << "[mrpt internalAssignImage_2D] Reusing existing textureName:" << get()->name
+                << "\n";
+
+    return;
+  }
+
   mrpt::img::CImage rgb;
 
   switch (in_rgb->getPixelDepth())
@@ -290,7 +346,7 @@ void Texture::internalAssignImage_2D(
   if (in_alpha) alpha = mrpt::img::CImage(*in_alpha, mrpt::img::SHALLOW_COPY);
 
   // allocate texture names:
-  get() = getNewTextureNumber();
+  get() = getNewTextureNumber(in_rgb->asCvMatRef().data);
   get()->unit = textureUnit;
 
   // activate the texture unit first before binding texture
@@ -536,7 +592,7 @@ void Texture::assignCubeImages(const std::array<mrpt::img::CImage, 6>& imgs, int
   }
 
   // allocate texture "name" (ID):
-  get() = getNewTextureNumber();
+  get() = getNewTextureNumber(nullptr); /* no cached img for cube textures */
 
   // activate the texture unit first before binding texture
   bindAsCubeTexture();

libs/slam/CMakeLists.txt

@@ -11,9 +11,8 @@ list(APPEND slam_EXTRA_SRCS_NAME
 	"slam" "slam" "slam-headers")
 
 if(CMAKE_MRPT_HAS_TBB)
-	set(tbb_dep TBB)
-else()
-	set(tbb_dep "")
+	set(EXTRA_CONFIG_CMDS
+		"find_dependency(TBB CONFIG)")
 endif()
 
 #---------------------------------------------
@@ -25,8 +24,6 @@ define_mrpt_lib(
 	# Dependencies
 	mrpt-vision
 	mrpt-maps
-	# Other imported targets:
-	${tbb_dep} # find_package() lib name
 	)
 
 if(BUILD_mrpt-slam)

parse-files/mrpt-xxx-config.cmake.in

@@ -34,6 +34,10 @@ if(NOT "${CMAKE_CURRENT_LIST_DIR}/../" IN_LIST CMAKE_PREFIX_PATH)
 endif()
 cmake_policy(POP)
 
+# Prefer config instead of the old FindXXX() files
+set(_BCK_CMAKE_FIND_PACKAGE_PREFER_CONFIG ${CMAKE_FIND_PACKAGE_PREFER_CONFIG})
+set(CMAKE_FIND_PACKAGE_PREFER_CONFIG TRUE)
+
 # Search for dependencies first:
 set(_deps "@ALL_DEPS_LIST@")
 foreach(_dep ${_deps}) # NO quotes for the list to be a CMake list!
@@ -47,5 +51,9 @@ foreach(_dep ${_deps}) # NO quotes for the list to be a CMake list!
 endforeach()
 @EXTRA_CONFIG_CMDS@
 
+# Restore user's settings:
+set(CMAKE_FIND_PACKAGE_PREFER_CONFIG ${_BCK_CMAKE_FIND_PACKAGE_PREFER_CONFIG})
+unset(_BCK_CMAKE_FIND_PACKAGE_PREFER_CONFIG)
+
 # Include targets for this library:
 include(${CMAKE_CURRENT_LIST_DIR}/mrpt-@[email protected])

python/src/mrpt/math/TBoundingBox.cpp

@@ -44,7 +44,7 @@ void bind_mrpt_math_TBoundingBox(std::function< pybind11::module &(std::string c
 		cl.def( pybind11::init( [](mrpt::math::TBoundingBox_<double> const &o){ return new mrpt::math::TBoundingBox_<double>(o); } ) );
 
 		pybind11::enum_<mrpt::math::TBoundingBox_<double>::CTOR_FLAGS>(cl, "CTOR_FLAGS", "")
-			.value("None", mrpt::math::TBoundingBox_<double>::CTOR_FLAGS::None)
+			.value("NoFlags", mrpt::math::TBoundingBox_<double>::CTOR_FLAGS::NoFlags)
 			.value("AllowUnordered", mrpt::math::TBoundingBox_<double>::CTOR_FLAGS::AllowUnordered);
 
 		cl.def_readwrite("min", &mrpt::math::TBoundingBox_<double>::min);
@@ -70,7 +70,7 @@ void bind_mrpt_math_TBoundingBox(std::function< pybind11::module &(std::string c
 		cl.def( pybind11::init( [](mrpt::math::TBoundingBox_<float> const &o){ return new mrpt::math::TBoundingBox_<float>(o); } ) );
 
 		pybind11::enum_<mrpt::math::TBoundingBox_<float>::CTOR_FLAGS>(cl, "CTOR_FLAGS", "")
-			.value("None", mrpt::math::TBoundingBox_<float>::CTOR_FLAGS::None)
+			.value("NoFlags", mrpt::math::TBoundingBox_<float>::CTOR_FLAGS::NoFlags)
 			.value("AllowUnordered", mrpt::math::TBoundingBox_<float>::CTOR_FLAGS::AllowUnordered);
 
 		cl.def_readwrite("min", &mrpt::math::TBoundingBox_<float>::min);