adding utility code from armarx::navigation

source/RobotAPI/libraries/ArmarXObjects/util.cpp

@@ -30,28 +30,93 @@
 
 #include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
 
-namespace armarx::objpose
+namespace armarx::objpose::util
 {
 
     objpose::ObjectPoseSeq
-    filterObjects(objpose::ObjectPoseSeq objects, const std::vector<std::string>& datasetBlocklist)
+    filterObjects(objpose::ObjectPoseSeq objects,
+                  const std::vector<std::string>& datasetDisableList)
     {
-        const auto isBlacklisted = [&datasetBlocklist](const objpose::ObjectPose& objectPose)
+        const auto isInDisableList = [&datasetDisableList](const objpose::ObjectPose& objectPose)
         {
             const auto dataset = objectPose.objectID.dataset();
 
-            return std::find(datasetBlocklist.begin(), datasetBlocklist.end(), dataset) !=
-                   datasetBlocklist.end();
+            return std::find(datasetDisableList.begin(), datasetDisableList.end(), dataset) !=
+                   datasetDisableList.end();
         };
 
-        objects.erase(std::remove_if(objects.begin(), objects.end(), isBlacklisted), objects.end());
+        objects.erase(std::remove_if(objects.begin(), objects.end(), isInDisableList),
+                      objects.end());
         return objects;
     }
 
+    objpose::ObjectPoseSeq
+    staticObjects(objpose::ObjectPoseSeq objects)
+    {
+        const auto isDynamic = [](const objpose::ObjectPose& objectPose)
+        { return not objectPose.isStatic; };
+
+        objects.erase(std::remove_if(objects.begin(), objects.end(), isDynamic), objects.end());
+        return objects;
+    }
+
+    objpose::ObjectPoseSeq
+    nonArticulatedObjects(objpose::ObjectPoseSeq objects)
+    {
+        ObjectFinder finder;
+        finder.setLogObjectDiscoveryError(false);
+
+        const auto isArticulated = [&finder](const objpose::ObjectPose& objectPose)
+        {
+            const auto objInfo = finder.findObject(objectPose.objectID);
+            ARMARX_CHECK_NOT_NULL(objInfo) << objectPose.objectID;
+            const bool hasArticulatedModel = objInfo->getArticulatedModel().has_value();
+            return hasArticulatedModel;
+        };
+
+        objects.erase(std::remove_if(objects.begin(), objects.end(), isArticulated), objects.end());
+        return objects;
+    }
+
+    objpose::ObjectPoseSeq
+    articulatedObjects(objpose::ObjectPoseSeq objects)
+    {
+        ObjectFinder finder;
+        finder.setLogObjectDiscoveryError(false);
+
+        const auto isNonArticulated = [&finder](const objpose::ObjectPose& objectPose)
+        {
+            const auto objInfo = finder.findObject(objectPose.objectID);
+            ARMARX_CHECK_NOT_NULL(objInfo) << objectPose.objectID;
+            const bool hasArticulatedModel = objInfo->getArticulatedModel().has_value();
+            return not hasArticulatedModel;
+        };
+
+        objects.erase(std::remove_if(objects.begin(), objects.end(), isNonArticulated),
+                      objects.end());
+        return objects;
+    }
+
+    std::optional<objpose::ObjectPose>
+    findObject(const objpose::ObjectPoseSeq& objectPoses, const armarx::ObjectID& objectID)
+    {
+        const auto matchesId = [&objectID](const objpose::ObjectPose& objectPose) -> bool
+        { return objectPose.objectID == objectID; };
+
+        const auto it = std::find_if(objectPoses.begin(), objectPoses.end(), matchesId);
+        if (it != objectPoses.end())
+        {
+            return *it;
+        }
+
+        return std::nullopt;
+    }
+
     VirtualRobot::ManipulationObjectPtr
     asManipulationObject(const objpose::ObjectPose& objectPose)
     {
         ObjectFinder finder;
+        finder.setLogObjectDiscoveryError(false);
 
         VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
         if (auto obstacle = finder.loadManipulationObject(objectPose))
@@ -68,10 +133,12 @@ namespace armarx::objpose
     asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
     {
         ObjectFinder finder;
+        finder.setLogObjectDiscoveryError(false);
 
         VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
         for (const auto& objectPose : objectPoses)
         {
+            ARMARX_VERBOSE << "Loading object `" << objectPose.objectID << "`";
             if (auto obstacle = finder.loadManipulationObject(objectPose))
             {
                 obstacle->setGlobalPose(objectPose.objectPoseGlobal);
@@ -82,5 +149,20 @@ namespace armarx::objpose
         return sceneObjects;
     }
 
+    VirtualRobot::SceneObjectPtr
+    asSceneObject(const objpose::ObjectPose& objectPose)
+    {
+        ObjectFinder finder;
+        finder.setLogObjectDiscoveryError(false);
+
+        VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+        if (auto obstacle = finder.loadManipulationObject(objectPose))
+        {
+            obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+            return obstacle;
+        }
+        return nullptr;
+    }
+
 
 } // namespace armarx::objpose

source/RobotAPI/libraries/ArmarXObjects/util.h

@@ -25,12 +25,22 @@
 
 #include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
 
-namespace armarx::objpose
+namespace armarx::objpose::util
 {
     objpose::ObjectPoseSeq filterObjects(objpose::ObjectPoseSeq objects,
-                                         const std::vector<std::string>& datasetBlocklist);
+                                         const std::vector<std::string>& datasetDisableList);
+
+    std::optional<objpose::ObjectPose> findObject(const objpose::ObjectPoseSeq& objectPoses,
+                                                  const armarx::ObjectID& objectID);
 
     VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose& objectPose);
     VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
 
+
+    objpose::ObjectPoseSeq staticObjects(objpose::ObjectPoseSeq objects);
+    objpose::ObjectPoseSeq nonArticulatedObjects(objpose::ObjectPoseSeq objects);
+    objpose::ObjectPoseSeq articulatedObjects(objpose::ObjectPoseSeq objects);
+
+    VirtualRobot::SceneObjectPtr asSceneObject(const objpose::ObjectPose& objectPose);
+
 } // namespace armarx::objpose

source/RobotAPI/libraries/armem_vision/types.h

@@ -38,7 +38,7 @@ namespace armarx::armem::vision
         float resolution; // [mm]
 
         std::string frame;
-        Eigen::Affine3f pose;
+        Eigen::Isometry3f pose;
 
         // using ValueType = _ValueT;
         using CellType = float;