util.cpp
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package Navigation::ArmarXObjects::util
17  * @author Fabian Reister ( fabian dot reister at kit dot edu )
18  * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
19  * @date 2021
20  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
21  * GNU General Public License
22  */
23 
24 #include "util.h"
25 
26 #include <string>
27 
28 #include <Eigen/Geometry>
29 
30 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
31 #include <VirtualRobot/ManipulationObject.h>
32 #include <VirtualRobot/Primitive.h>
33 #include <VirtualRobot/SceneObjectSet.h>
34 #include <VirtualRobot/VirtualRobot.h>
35 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
36 #include <VirtualRobot/Visualization/VisualizationNode.h>
37 
39 
44 
46 {
47 
49  filterObjects(objpose::ObjectPoseSeq objects, const std::vector<std::string>& datasetBlacklist)
50  {
51  const auto isBlacklisted = [&datasetBlacklist](const objpose::ObjectPose& objectPose)
52  {
53  const auto dataset = objectPose.objectID.dataset();
54 
55  return std::find(datasetBlacklist.begin(), datasetBlacklist.end(), dataset) !=
56  datasetBlacklist.end();
57  };
58 
59  objects.erase(std::remove_if(objects.begin(), objects.end(), isBlacklisted), objects.end());
60  return objects;
61  }
62 
65  {
66  const auto isDynamic = [](const objpose::ObjectPose& objectPose)
67  { return not objectPose.isStatic; };
68 
69  objects.erase(std::remove_if(objects.begin(), objects.end(), isDynamic), objects.end());
70  return objects;
71  }
72 
73  std::optional<objpose::ObjectPose>
74  findObject(const objpose::ObjectPoseSeq& objectPoses, const armarx::ObjectID& objectID)
75  {
76  const auto matchesId = [&objectID](const objpose::ObjectPose& objectPose) -> bool
77  { return objectPose.objectID == objectID; };
78 
79  const auto it = std::find_if(objectPoses.begin(), objectPoses.end(), matchesId);
80  if (it != objectPoses.end())
81  {
82  return *it;
83  }
84 
85  return std::nullopt;
86  }
87 
88  VirtualRobot::ManipulationObjectPtr
90  {
91  ObjectFinder finder;
92  finder.setLogObjectDiscoveryError(false);
93 
94  VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
95  if (auto obstacle = finder.loadManipulationObject(objectPose))
96  {
97  obstacle->setGlobalPose(objectPose.objectPoseGlobal);
98  return obstacle;
99  }
100 
101  ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
102  return nullptr;
103  }
104 
105  VirtualRobot::SceneObjectSetPtr
107  {
108  ObjectFinder finder;
109  finder.setLogObjectDiscoveryError(false);
110 
111  VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
112  for (const auto& objectPose : objectPoses)
113  {
114  if (auto obstacle = finder.loadManipulationObject(objectPose))
115  {
116  obstacle->setGlobalPose(objectPose.objectPoseGlobal);
117  sceneObjects->addSceneObject(obstacle);
118  }
119  }
120 
121  return sceneObjects;
122  }
123 
126  {
127  ObjectFinder finder;
128  finder.setLogObjectDiscoveryError(false);
129 
130  VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
131  if (auto obstacle = finder.loadManipulationObject(objectPose))
132  {
133  obstacle->setGlobalPose(objectPose.objectPoseGlobal);
134  return obstacle;
135  }
136  return nullptr;
137  }
138 
139  VirtualRobot::SceneObjectSetPtr
141  const OccupancyGridHelper::Params& params)
142  {
143  const OccupancyGridHelper ocHelper(occupancyGrid, params);
144  const auto obstacles = ocHelper.obstacles();
145 
146  const float boxSize = occupancyGrid.resolution;
147  const float resolution = occupancyGrid.resolution;
148 
149  VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
150 
151  ARMARX_CHECK_EQUAL(occupancyGrid.frame, GlobalFrame)
152  << "Only occupancy grid in global frame supported.";
153 
154  VirtualRobot::CoinVisualizationFactory factory;
155 
156  const auto& world_T_map = occupancyGrid.pose;
157 
158  for (int x = 0; x < obstacles.rows(); x++)
159  {
160  for (int y = 0; y < obstacles.cols(); y++)
161  {
162  if (obstacles(x, y))
163  {
164  const Eigen::Vector3f pos{
165  static_cast<float>(x * resolution), static_cast<float>(y * resolution), 0};
166 
167  // FIXME: change to Isometry3f
168  Eigen::Affine3f map_T_obj = Eigen::Affine3f::Identity();
169  map_T_obj.translation() = pos;
170 
171  Eigen::Affine3f world_T_obj = world_T_map * map_T_obj;
172 
173  // ARMARX_INFO << world_T_obj.translation();
174 
175  auto cube = factory.createBox(boxSize, boxSize, boxSize);
176 
177  VirtualRobot::CollisionModelPtr collisionModel(
178  new VirtualRobot::CollisionModel(cube));
179 
180  VirtualRobot::SceneObjectPtr sceneObject(new VirtualRobot::SceneObject(
181  "box_" + std::to_string(sceneObjects->getSize()), cube, collisionModel));
182  sceneObject->setGlobalPose(world_T_obj.matrix());
183 
184  sceneObjects->addSceneObject(sceneObject);
185  }
186  }
187  }
188 
189  return sceneObjects;
190  }
191 } // namespace armarx::navigation::util
armarx::navigation::util::filterObjects
objpose::ObjectPoseSeq filterObjects(objpose::ObjectPoseSeq objects, const std::vector< std::string > &datasetBlacklist)
Definition: util.cpp:49
armarx::ObjectID
A known object ID of the form "Dataset/ClassName" or "Dataset/ClassName/InstanceName".
Definition: ObjectID.h:11
armarx::armem::vision::OccupancyGrid
Definition: types.h:36
armarx::navigation::util
This file is part of ArmarX.
Definition: geometry.cpp:8
armarx::objpose::ObjectPoseSeq
std::vector< ObjectPose > ObjectPoseSeq
Definition: forward_declarations.h:20
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::OccupancyGridHelper
Definition: OccupancyGridHelper.h:23
armarx::ObjectFinder::loadManipulationObject
static VirtualRobot::ManipulationObjectPtr loadManipulationObject(const std::optional< ObjectInfo > &ts)
Definition: ObjectFinder.cpp:298
armarx::ObjectFinder::setLogObjectDiscoveryError
void setLogObjectDiscoveryError(bool logEnabled)
Definition: ObjectFinder.cpp:377
armarx::armem::vision::OccupancyGrid::frame
std::string frame
Definition: types.h:40
armarx::armem::vision::OccupancyGrid::pose
Eigen::Affine3f pose
Definition: types.h:41
armarx::navigation::util::asSceneObject
VirtualRobot::SceneObjectPtr asSceneObject(const objpose::ObjectPose &objectPose)
Definition: util.cpp:125
armarx::ObjectFinder
Used to find objects in the ArmarX objects repository [1] (formerly [2]).
Definition: ObjectFinder.h:23
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
util.h
FramedPose.h
armarx::navigation::util::findObject
std::optional< objpose::ObjectPose > findObject(const objpose::ObjectPoseSeq &objectPoses, const armarx::ObjectID &objectID)
Definition: util.cpp:74
armarx::detail::OccupancyGridHelperParams
Definition: OccupancyGridHelper.h:16
armarx::navigation::util::asSceneObjects
VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq &objectPoses)
Definition: util.cpp:106
armarx::OccupancyGridHelper::obstacles
BinaryArray obstacles() const
Definition: OccupancyGridHelper.cpp:31
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::navigation::util::asManipulationObject
VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose &objectPose)
Definition: util.cpp:89
armarx::armem::vision::OccupancyGrid::resolution
float resolution
Definition: types.h:38
ExpressionException.h
forward_declarations.h
armarx::objpose::ObjectPose::objectID
armarx::ObjectID objectID
The object ID, i.e. dataset, class name and instance name.
Definition: ObjectPose.h:58
armarx::objpose::ObjectPose::objectPoseGlobal
Eigen::Matrix4f objectPoseGlobal
The object pose in the global frame.
Definition: ObjectPose.h:73
OccupancyGridHelper.h
armarx::navigation::util::staticObjects
objpose::ObjectPoseSeq staticObjects(objpose::ObjectPoseSeq objects)
Definition: util.cpp:64
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::objpose::ObjectPose
An object pose as stored by the ObjectPoseStorage.
Definition: ObjectPose.h:36
ObjectFinder.h