test_body_operations.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, Bielefeld University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Bielefeld University nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include <geometric_shapes/body_operations.h>
36 #include <geometric_shapes/shape_operations.h>
37 #include <boost/filesystem.hpp>
38 #include "resources/config.h"
39 #include <gtest/gtest.h>
40 
41 using namespace bodies;
42 
43 void expectVector3dSetsEqual(EigenSTL::vector_Vector3d vec1, EigenSTL::vector_Vector3d vec2,
44  const double upToError = 1e-6)
45 {
46  ASSERT_EQ(vec1.size(), vec2.size());
47 
48  auto vecCompare = [upToError](const Eigen::Vector3d& a, const Eigen::Vector3d& b) -> bool {
49  if (a.x() < b.x() - upToError)
50  return true;
51  if (a.x() > b.x() + upToError)
52  return false;
53 
54  if (a.y() < b.y() - upToError)
55  return true;
56  if (a.y() > b.y() + upToError)
57  return false;
58 
59  if (a.z() < b.z() - upToError)
60  return true;
61  if (a.z() > b.z() + upToError)
62  return false;
63 
64  return false;
65  };
66 
67  std::sort(vec1.begin(), vec1.end(), vecCompare);
68  std::sort(vec2.begin(), vec2.end(), vecCompare);
69 
70  for (size_t i = 0; i < vec1.size(); ++i)
71  {
72  EXPECT_NEAR(vec1[i].x(), vec2[i].x(), upToError);
73  EXPECT_NEAR(vec1[i].y(), vec2[i].y(), upToError);
74  EXPECT_NEAR(vec1[i].z(), vec2[i].z(), upToError);
75  }
76 }
77 
78 TEST(Bodies, ConstructShapeFromBodySphere)
79 {
80  const shapes::Shape* shape = new shapes::Sphere(2.0);
81  const auto body = new Sphere(shape);
82 
83  const auto constructedShape = constructShapeFromBody(body);
84  const auto constructedSphere = std::dynamic_pointer_cast<const shapes::Sphere>(constructedShape);
85 
86  EXPECT_EQ(shape->type, constructedShape->type);
87  ASSERT_NE(nullptr, constructedSphere);
88  EXPECT_EQ(2.0, constructedSphere->radius);
89 }
90 
91 TEST(Bodies, ConstructShapeFromBodyBox)
92 {
93  const shapes::Shape* shape = new shapes::Box(1.0, 2.0, 3.0);
94  const auto body = new Box(shape);
95 
96  const auto constructedShape = constructShapeFromBody(body);
97  const auto constructedBox = std::dynamic_pointer_cast<const shapes::Box>(constructedShape);
98 
99  EXPECT_EQ(shape->type, constructedShape->type);
100  ASSERT_NE(nullptr, constructedBox);
101  EXPECT_EQ(1.0, constructedBox->size[0]);
102  EXPECT_EQ(2.0, constructedBox->size[1]);
103  EXPECT_EQ(3.0, constructedBox->size[2]);
104 }
105 
106 TEST(Bodies, ConstructShapeFromBodyCylinder)
107 {
108  const shapes::Shape* shape = new shapes::Cylinder(1.0, 2.0);
109  const auto body = new Cylinder(shape);
110 
111  const auto constructedShape = constructShapeFromBody(body);
112  const auto constructedCylinder = std::dynamic_pointer_cast<const shapes::Cylinder>(constructedShape);
113 
114  EXPECT_EQ(shape->type, constructedShape->type);
115  ASSERT_NE(nullptr, constructedCylinder);
116  EXPECT_EQ(1.0, constructedCylinder->radius);
117  EXPECT_EQ(2.0, constructedCylinder->length);
118 }
119 
120 TEST(Bodies, ConstructShapeFromBodyMesh)
121 {
122  shapes::Mesh* shape =
123  shapes::createMeshFromResource("file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/box.dae").string());
124  const auto body = new ConvexMesh(shape);
125 
126  const auto constructedShape = constructShapeFromBody(body);
127  const auto constructedMesh = std::dynamic_pointer_cast<const shapes::Mesh>(constructedShape);
128 
129  EXPECT_EQ(shape->type, constructedShape->type);
130  ASSERT_NE(nullptr, constructedMesh);
131  ASSERT_EQ(shape->vertex_count, constructedMesh->vertex_count);
132  ASSERT_EQ(shape->triangle_count, constructedMesh->triangle_count);
133 
134  // Compare the vertices and triangle normals of the constructed mesh
135  // Triangle indices cannot be checked because the vertex IDs could change in the constructed mesh
136 
137  EigenSTL::vector_Vector3d verticesOrig, verticesConstructed;
138  for (size_t i = 0; i < shape->vertex_count * 3; i += 3)
139  verticesOrig.push_back({ shape->vertices[i], shape->vertices[i + 1], shape->vertices[i + 2] });
140  for (size_t i = 0; i < constructedMesh->vertex_count * 3; i += 3)
141  verticesConstructed.push_back(
142  { constructedMesh->vertices[i], constructedMesh->vertices[i + 1], constructedMesh->vertices[i + 2] });
143 
144  expectVector3dSetsEqual(verticesOrig, verticesConstructed);
145 
146  EigenSTL::vector_Vector3d normalsOrig, normalsConstructed;
147  shape->computeTriangleNormals();
148  // constructedMesh->computeTriangleNormals(); // is done during construction
149  for (size_t i = 0; i < shape->triangle_count * 3; i += 3)
150  normalsOrig.push_back(
151  { shape->triangle_normals[i], shape->triangle_normals[i + 1], shape->triangle_normals[i + 2] });
152  for (size_t i = 0; i < constructedMesh->triangle_count * 3; i += 3)
153  normalsConstructed.push_back({ constructedMesh->triangle_normals[i], constructedMesh->triangle_normals[i + 1],
154  constructedMesh->triangle_normals[i + 2] });
155 
156  expectVector3dSetsEqual(normalsOrig, normalsConstructed);
157 }
158 
159 TEST(Bodies, ConstructMarkerFromBodySphere)
160 {
161  const Eigen::Isometry3d pose =
162  Eigen::Translation3d(1.0, 2.0, 3.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0.0, 1.0, 0.0));
163 
164  const shapes::Shape* shape = new shapes::Sphere(2.0);
165  const auto body = new Sphere(shape);
166  body->setPose(pose);
167 
168  visualization_msgs::Marker marker;
169  constructMarkerFromBody(body, marker);
170 
171  EXPECT_EQ(visualization_msgs::Marker::SPHERE, marker.type);
172  EXPECT_DOUBLE_EQ(4.0, marker.scale.x);
173  EXPECT_DOUBLE_EQ(4.0, marker.scale.y);
174  EXPECT_DOUBLE_EQ(4.0, marker.scale.z);
175  EXPECT_DOUBLE_EQ(1.0, marker.pose.position.x);
176  EXPECT_DOUBLE_EQ(2.0, marker.pose.position.y);
177  EXPECT_DOUBLE_EQ(3.0, marker.pose.position.z);
178  EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.x);
179  EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.y);
180  EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.z);
181  EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.w);
182 }
183 
184 TEST(Bodies, ConstructMarkerFromBodyBox)
185 {
186  const Eigen::Isometry3d pose =
187  Eigen::Translation3d(1.0, 2.0, 3.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0.0, 1.0, 0.0));
188 
189  const shapes::Shape* shape = new shapes::Box(1.0, 2.0, 3.0);
190  const auto body = new Box(shape);
191  body->setPose(pose);
192 
193  visualization_msgs::Marker marker;
194  constructMarkerFromBody(body, marker);
195 
196  EXPECT_EQ(visualization_msgs::Marker::CUBE, marker.type);
197  EXPECT_DOUBLE_EQ(1.0, marker.scale.x);
198  EXPECT_DOUBLE_EQ(2.0, marker.scale.y);
199  EXPECT_DOUBLE_EQ(3.0, marker.scale.z);
200  EXPECT_DOUBLE_EQ(1.0, marker.pose.position.x);
201  EXPECT_DOUBLE_EQ(2.0, marker.pose.position.y);
202  EXPECT_DOUBLE_EQ(3.0, marker.pose.position.z);
203  EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.x);
204  EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.y);
205  EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.z);
206  EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.w);
207 }
208 
209 TEST(Bodies, ConstructMarkerFromBodyCylinder)
210 {
211  const Eigen::Isometry3d pose =
212  Eigen::Translation3d(1.0, 2.0, 3.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0.0, 1.0, 0.0));
213 
214  const shapes::Shape* shape = new shapes::Cylinder(3.0, 2.0);
215  const auto body = new Cylinder(shape);
216  body->setPose(pose);
217 
218  visualization_msgs::Marker marker;
219  constructMarkerFromBody(body, marker);
220 
221  EXPECT_EQ(visualization_msgs::Marker::CYLINDER, marker.type);
222  EXPECT_DOUBLE_EQ(6.0, marker.scale.x);
223  EXPECT_DOUBLE_EQ(6.0, marker.scale.y);
224  EXPECT_DOUBLE_EQ(2.0, marker.scale.z);
225  EXPECT_DOUBLE_EQ(1.0, marker.pose.position.x);
226  EXPECT_DOUBLE_EQ(2.0, marker.pose.position.y);
227  EXPECT_DOUBLE_EQ(3.0, marker.pose.position.z);
228  EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.x);
229  EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.y);
230  EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.z);
231  EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.w);
232 }
233 
234 TEST(Bodies, ConstructMarkerFromBodyMesh)
235 {
236  const Eigen::Isometry3d pose =
237  Eigen::Translation3d(1.0, 2.0, 3.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0.0, 1.0, 0.0));
238 
239  shapes::Mesh* shape =
240  shapes::createMeshFromResource("file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) / "/box.dae").string());
241  const auto body = new ConvexMesh(shape);
242  body->setPose(pose);
243 
244  visualization_msgs::Marker marker;
245  constructMarkerFromBody(body, marker);
246 
247  EXPECT_EQ(visualization_msgs::Marker::TRIANGLE_LIST, marker.type);
248  EXPECT_DOUBLE_EQ(1.0, marker.scale.x);
249  EXPECT_DOUBLE_EQ(1.0, marker.scale.y);
250  EXPECT_DOUBLE_EQ(1.0, marker.scale.z);
251  EXPECT_DOUBLE_EQ(1.0, marker.pose.position.x);
252  EXPECT_DOUBLE_EQ(2.0, marker.pose.position.y);
253  EXPECT_DOUBLE_EQ(3.0, marker.pose.position.z);
254  EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.x);
255  EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.y);
256  EXPECT_DOUBLE_EQ(0.0, marker.pose.orientation.z);
257  EXPECT_DOUBLE_EQ(M_SQRT1_2, marker.pose.orientation.w);
258 
259  // We can't directly use shape->triangles because after passing it to the body constructor,
260  // it can "optimize" the vertices (i.e. swap normals etc.) or reorder them
261  const auto shapeFromBody = std::dynamic_pointer_cast<const shapes::Mesh>(constructShapeFromBody(body));
262 
263  EigenSTL::vector_Vector3d shapeVertices, markerVertices;
264  for (size_t t = 0; t < shapeFromBody->triangle_count * 3; ++t)
265  {
266  const auto vertexId = shapeFromBody->triangles[t];
267  shapeVertices.push_back({ shapeFromBody->vertices[3 * vertexId + 0], shapeFromBody->vertices[3 * vertexId + 1],
268  shapeFromBody->vertices[3 * vertexId + 2] });
269  }
270  for (auto& point : marker.points)
271  markerVertices.push_back({ point.x, point.y, point.z });
272 
273  expectVector3dSetsEqual(shapeVertices, markerVertices);
274 }
275 
276 int main(int argc, char** argv)
277 {
278  testing::InitGoogleTest(&argc, argv);
279  return RUN_ALL_TESTS();
280 }
bodies::Sphere
Definition of a sphere.
Definition: bodies.h:327
main
int main(int argc, char **argv)
Definition: test_body_operations.cpp:276
bodies::constructMarkerFromBody
void constructMarkerFromBody(const bodies::Body *body, visualization_msgs::Marker &msg)
Construct a Marker message that corresponds to this (scaled and padded) body.
Definition: body_operations.cpp:130
shapes::Cylinder
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
shapes::Mesh::triangle_count
unsigned int triangle_count
The number of triangles formed with the vertices.
Definition: shapes.h:350
expectVector3dSetsEqual
void expectVector3dSetsEqual(EigenSTL::vector_Vector3d vec1, EigenSTL::vector_Vector3d vec2, const double upToError=1e-6)
Definition: test_body_operations.cpp:43
shapes::SPHERE
@ SPHERE
Definition: shapes.h:64
shapes::Shape
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin....
Definition: shapes.h:281
body_operations.h
shapes::Box
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
shapes::Sphere
Definition of a sphere.
Definition: shapes.h:106
shapes::Mesh::vertex_count
unsigned int vertex_count
The number of available vertices.
Definition: shapes.h:343
y
double y
Definition: mesh_operations.cpp:202
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
EigenSTL::vector_Vector3d
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
M_PI_2
#define M_PI_2
bodies::Box
Definition of a box.
Definition: bodies.h:443
shapes::Shape::type
ShapeType type
The type of the shape.
Definition: shapes.h:102
shapes::Mesh::computeTriangleNormals
void computeTriangleNormals()
Compute the normals of each triangle from its vertices via cross product.
Definition: shapes.cpp:535
shapes::createMeshFromResource
Mesh * createMeshFromResource(const std::string &resource)
Load a mesh from a resource that contains a mesh that can be loaded by assimp.
Definition: mesh_operations.cpp:234
bodies::constructShapeFromBody
shapes::ShapeConstPtr constructShapeFromBody(const bodies::Body *body)
Get a shape that corresponds to this (scaled and padded) body.
Definition: body_operations.cpp:81
point
std::chrono::system_clock::time_point point
shape_operations.h
bodies
This set of classes allows quickly detecting whether a given point is inside an object or not....
Definition: aabb.h:42
TEST
TEST(Bodies, ConstructShapeFromBodySphere)
Definition: test_body_operations.cpp:78
shapes::CYLINDER
@ CYLINDER
Definition: shapes.h:65
x
double x
Definition: mesh_operations.cpp:202
bodies::Cylinder
Definition of a cylinder.
Definition: bodies.h:380
bodies::ConvexMesh
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:506
z
double z
Definition: mesh_operations.cpp:202
EXPECT_EQ
#define EXPECT_EQ(a, b)

geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Sun Oct 1 2023 02:40:16