test_bounding_cylinder.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, Open Robotics.
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 Open Robotics 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 
37 #include <geometric_shapes/bodies.h>
38 #include <geometric_shapes/body_operations.h>
39 #include <random_numbers/random_numbers.h>
40 #include <gtest/gtest.h>
41 
42 // The magic numbers in this test were verified visually using Blender
43 
44 TEST(SphereBoundingCylinder, Sphere1)
45 {
46  shapes::Sphere shape(1.0);
47  bodies::Sphere body(&shape);
48  bodies::BoundingCylinder bcyl;
49  body.computeBoundingCylinder(bcyl);
50 
51  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
52  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
53  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
54  // orientation can be any
55  EXPECT_NEAR(1.0, bcyl.radius, 1e-4);
56  EXPECT_NEAR(2.0, bcyl.length, 1e-4);
57 
58  body.setScale(2.0);
59  body.setPadding(1.0);
60  body.computeBoundingCylinder(bcyl);
61 
62  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
63  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
64  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
65  // orientation can be any
66  EXPECT_NEAR(3.0, bcyl.radius, 1e-4);
67  EXPECT_NEAR(6.0, bcyl.length, 1e-4);
68 }
69 
70 TEST(SphereBoundingCylinder, Sphere2)
71 {
72  shapes::Sphere shape(2.0);
73  bodies::Sphere body(&shape);
74  Eigen::Isometry3d pose(Eigen::Isometry3d::TranslationType(1, 2, 3));
75  body.setPose(pose);
76  bodies::BoundingCylinder bcyl;
77  body.computeBoundingCylinder(bcyl);
78 
79  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
80  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
81  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
82  // orientation can be any
83  EXPECT_NEAR(2.0, bcyl.radius, 1e-4);
84  EXPECT_NEAR(4.0, bcyl.length, 1e-4);
85 
86  // verify the bounding cylinder is rotation-invariant
87 
88  pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
89  body.setPose(pose);
90  body.computeBoundingCylinder(bcyl);
91 
92  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
93  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
94  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
95  // orientation can be any
96  EXPECT_NEAR(2.0, bcyl.radius, 1e-4);
97  EXPECT_NEAR(4.0, bcyl.length, 1e-4);
98 
99  random_numbers::RandomNumberGenerator gen;
100  double quatData[4];
101  Eigen::Quaterniond quat;
102 
103  for (size_t i = 0; i < 10; ++i)
104  {
105  gen.quaternion(quatData);
106  quat.x() = quatData[0];
107  quat.y() = quatData[1];
108  quat.z() = quatData[2];
109  quat.w() = quatData[3];
110  pose.linear() = quat.toRotationMatrix();
111  body.setPose(pose);
112  bodies::BoundingCylinder bcyl2;
113  body.computeBoundingCylinder(bcyl2);
114 
115  EXPECT_NEAR(1.0, bcyl2.pose.translation().x(), 1e-4);
116  EXPECT_NEAR(2.0, bcyl2.pose.translation().y(), 1e-4);
117  EXPECT_NEAR(3.0, bcyl2.pose.translation().z(), 1e-4);
118  // orientation can be any
119  EXPECT_NEAR(2.0, bcyl2.radius, 1e-4);
120  EXPECT_NEAR(4.0, bcyl2.length, 1e-4);
121  }
122 }
123 
124 TEST(BoxBoundingCylinder, Box1)
125 {
126  shapes::Box shape(1.0, 2.0, 3.0);
127  bodies::Box body(&shape);
128  bodies::BoundingCylinder bcyl;
129  body.computeBoundingCylinder(bcyl);
130 
131  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
132  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
133  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
134  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
135  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
136  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
137  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
138  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
139  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
140 
141  body.setScale(2.0);
142  body.setPadding(1.0);
143  body.computeBoundingCylinder(bcyl);
144 
145  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
146  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
147  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
148  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
149  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
150  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
151  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
152  EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.radius, 1e-4);
153  EXPECT_NEAR(8.0, bcyl.length, 1e-4);
154 
155  // test that the rotational axis of the cylinder sticks with the longest dimension
156 
157  shape = shapes::Box(2.0, 3.0, 1.0);
158  body = bodies::Box(&shape);
159  body.computeBoundingCylinder(bcyl);
160 
161  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
162  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
163  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
164  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
165  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
166  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
167  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
168  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
169  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
170 
171  body.setScale(2.0);
172  body.setPadding(1.0);
173  body.computeBoundingCylinder(bcyl);
174 
175  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
176  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
177  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
178  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
179  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
180  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
181  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
182  EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.radius, 1e-4);
183  EXPECT_NEAR(8.0, bcyl.length, 1e-4);
184 
185  shape = shapes::Box(3.0, 1.0, 2.0);
186  body = bodies::Box(&shape);
187  body.computeBoundingCylinder(bcyl);
188 
189  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
190  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
191  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
192  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
193  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
194  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
195  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
196  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
197  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
198 
199  body.setScale(2.0);
200  body.setPadding(1.0);
201  body.computeBoundingCylinder(bcyl);
202 
203  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
204  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
205  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
206  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
207  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
208  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
209  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
210  EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.radius, 1e-4);
211  EXPECT_NEAR(8.0, bcyl.length, 1e-4);
212 }
213 
214 TEST(BoxBoundingCylinder, Box2)
215 {
216  shapes::Box shape(1.0, 2.0, 3.0);
217  bodies::Box body(&shape);
218  Eigen::Isometry3d pose;
219  pose.setIdentity();
220  pose.translation() = Eigen::Vector3d(1, 2, 3);
221  body.setPose(pose);
222  bodies::BoundingCylinder bcyl;
223  body.computeBoundingCylinder(bcyl);
224 
225  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
226  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
227  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
228  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
229  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
230  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
231  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
232  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
233  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
234 
235  Eigen::AngleAxisd rot(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
236  pose *= rot;
237  body.setPose(pose);
238  body.computeBoundingCylinder(bcyl);
239 
240  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
241  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
242  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
243  EXPECT_NEAR(Eigen::Quaterniond(rot).x(), Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
244  EXPECT_NEAR(Eigen::Quaterniond(rot).y(), Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
245  EXPECT_NEAR(Eigen::Quaterniond(rot).z(), Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
246  EXPECT_NEAR(Eigen::Quaterniond(rot).w(), Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
247  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
248  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
249 }
250 
251 TEST(CylinderBoundingCylinder, Cylinder1)
252 {
253  shapes::Cylinder shape(1.0, 2.0);
254  bodies::Cylinder body(&shape);
255  bodies::BoundingCylinder bcyl;
256  body.computeBoundingCylinder(bcyl);
257 
258  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
259  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
260  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
261  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
262  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
263  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
264  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
265  EXPECT_NEAR(1.0, bcyl.radius, 1e-4);
266  EXPECT_NEAR(2.0, bcyl.length, 1e-4);
267 
268  body.setScale(2.0);
269  body.setPadding(1.0);
270  body.computeBoundingCylinder(bcyl);
271 
272  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
273  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
274  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
275  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
276  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
277  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
278  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
279  EXPECT_NEAR(3.0, bcyl.radius, 1e-4);
280  EXPECT_NEAR(6.0, bcyl.length, 1e-4);
281 }
282 
283 TEST(CylinderBoundingCylinder, Cylinder2)
284 {
285  shapes::Cylinder shape(1.0, 2.0);
286  bodies::Cylinder body(&shape);
287  Eigen::Isometry3d pose;
288  pose.setIdentity();
289  pose.translation() = Eigen::Vector3d(1, 2, 3);
290  body.setPose(pose);
291  bodies::BoundingCylinder bcyl;
292  body.computeBoundingCylinder(bcyl);
293 
294  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
295  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
296  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
297  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
298  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
299  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
300  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
301  EXPECT_NEAR(1.0, bcyl.radius, 1e-4);
302  EXPECT_NEAR(2.0, bcyl.length, 1e-4);
303 
304  pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).toRotationMatrix();
305  body.setPose(pose);
306  body.computeBoundingCylinder(bcyl);
307 
308  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
309  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
310  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
311  EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).x(), Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
312  EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).y(), Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
313  EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).z(), Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
314  EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).w(), Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
315  EXPECT_NEAR(1.0, bcyl.radius, 1e-4);
316  EXPECT_NEAR(2.0, bcyl.length, 1e-4);
317 }
318 
319 shapes::Mesh* createBoxMesh(const Eigen::Vector3d& min, const Eigen::Vector3d& max)
320 {
321  shapes::Mesh* m = new shapes::Mesh(8, 12);
322 
323  m->vertices[3 * 0 + 0] = min.x();
324  m->vertices[3 * 0 + 1] = min.y();
325  m->vertices[3 * 0 + 2] = min.z();
326 
327  m->vertices[3 * 1 + 0] = max.x();
328  m->vertices[3 * 1 + 1] = min.y();
329  m->vertices[3 * 1 + 2] = min.z();
330 
331  m->vertices[3 * 2 + 0] = min.x();
332  m->vertices[3 * 2 + 1] = max.y();
333  m->vertices[3 * 2 + 2] = min.z();
334 
335  m->vertices[3 * 3 + 0] = max.x();
336  m->vertices[3 * 3 + 1] = max.y();
337  m->vertices[3 * 3 + 2] = min.z();
338 
339  m->vertices[3 * 4 + 0] = min.x();
340  m->vertices[3 * 4 + 1] = min.y();
341  m->vertices[3 * 4 + 2] = max.z();
342 
343  m->vertices[3 * 5 + 0] = max.x();
344  m->vertices[3 * 5 + 1] = min.y();
345  m->vertices[3 * 5 + 2] = max.z();
346 
347  m->vertices[3 * 6 + 0] = min.x();
348  m->vertices[3 * 6 + 1] = max.y();
349  m->vertices[3 * 6 + 2] = max.z();
350 
351  m->vertices[3 * 7 + 0] = max.x();
352  m->vertices[3 * 7 + 1] = max.y();
353  m->vertices[3 * 7 + 2] = max.z();
354 
355  m->triangles[3 * 0 + 0] = 0;
356  m->triangles[3 * 0 + 1] = 1;
357  m->triangles[3 * 0 + 2] = 2;
358 
359  m->triangles[3 * 1 + 0] = 1;
360  m->triangles[3 * 1 + 1] = 3;
361  m->triangles[3 * 1 + 2] = 2;
362 
363  m->triangles[3 * 2 + 0] = 5;
364  m->triangles[3 * 2 + 1] = 4;
365  m->triangles[3 * 2 + 2] = 6;
366 
367  m->triangles[3 * 3 + 0] = 5;
368  m->triangles[3 * 3 + 1] = 6;
369  m->triangles[3 * 3 + 2] = 7;
370 
371  m->triangles[3 * 4 + 0] = 1;
372  m->triangles[3 * 4 + 1] = 5;
373  m->triangles[3 * 4 + 2] = 3;
374 
375  m->triangles[3 * 5 + 0] = 5;
376  m->triangles[3 * 5 + 1] = 7;
377  m->triangles[3 * 5 + 2] = 3;
378 
379  m->triangles[3 * 6 + 0] = 4;
380  m->triangles[3 * 6 + 1] = 0;
381  m->triangles[3 * 6 + 2] = 2;
382 
383  m->triangles[3 * 7 + 0] = 4;
384  m->triangles[3 * 7 + 1] = 2;
385  m->triangles[3 * 7 + 2] = 6;
386 
387  m->triangles[3 * 8 + 0] = 2;
388  m->triangles[3 * 8 + 1] = 3;
389  m->triangles[3 * 8 + 2] = 6;
390 
391  m->triangles[3 * 9 + 0] = 3;
392  m->triangles[3 * 9 + 1] = 7;
393  m->triangles[3 * 9 + 2] = 6;
394 
395  m->triangles[3 * 10 + 0] = 1;
396  m->triangles[3 * 10 + 1] = 0;
397  m->triangles[3 * 10 + 2] = 4;
398 
399  m->triangles[3 * 11 + 0] = 1;
400  m->triangles[3 * 11 + 1] = 4;
401  m->triangles[3 * 11 + 2] = 5;
402 
403  return m;
404 }
405 
406 TEST(MeshBoundingCylinder, Mesh1)
407 {
408  shapes::Mesh* m = createBoxMesh({ -0.5, -1.0, -1.5 }, { 0.5, 1.0, 1.5 });
409 
410  double radiusScaled;
411  double lengthScaled;
412 
413  bodies::ConvexMesh body(m);
414  bodies::BoundingCylinder bcyl;
415  body.computeBoundingCylinder(bcyl);
416 
417  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
418  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
419  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
420  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
421  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
422  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
423  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
424  // for meshes without padding, the bounding cylinder should be tight
425  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
426  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
427 
428  body.setScale(2.0);
429  body.setPadding(1.0);
430  body.computeBoundingCylinder(bcyl);
431 
432  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
433  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
434  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
435  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
436  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
437  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
438  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
439  // computation of bounding cylinder with padding is only approximate; the approximation error in both radius and
440  // length is anywhere from zero to padding_ (or 2*padding_ for length)
441  radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1);
442  EXPECT_LE(radiusScaled, bcyl.radius);
443  EXPECT_GE(radiusScaled + 1.0, bcyl.radius);
444  lengthScaled = 6.0;
445  EXPECT_LE(lengthScaled, bcyl.length);
446  EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.length);
447 
448  delete m;
449 
450  // test that the rotational axis of the cylinder sticks with the longest dimension
451 
452  m = createBoxMesh({ -1.0, -1.5, -0.5 }, { 1.0, 1.5, 0.5 });
453  bodies::ConvexMesh body2(m);
454  body2.computeBoundingCylinder(bcyl);
455 
456  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
457  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
458  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
459  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
460  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
461  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
462  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
463  // for meshes without padding, the bounding cylinder should be tight
464  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
465  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
466 
467  body2.setScale(2.0);
468  body2.setPadding(1.0);
469  body2.computeBoundingCylinder(bcyl);
470 
471  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
472  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
473  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
474  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
475  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
476  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
477  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
478  radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1);
479  EXPECT_LE(radiusScaled, bcyl.radius);
480  EXPECT_GE(radiusScaled + 1.0, bcyl.radius);
481  lengthScaled = 6.0;
482  EXPECT_LE(lengthScaled, bcyl.length);
483  EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.length);
484 
485  delete m;
486 
487  m = createBoxMesh({ -1.5, -0.5, -1.0 }, { 1.5, 0.5, 1.0 });
488  bodies::ConvexMesh body3(m);
489  body3.computeBoundingCylinder(bcyl);
490 
491  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
492  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
493  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
494  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
495  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
496  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
497  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
498  // for meshes without padding, the bounding cylinder should be tight
499  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
500  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
501 
502  body3.setScale(2.0);
503  body3.setPadding(1.0);
504  body3.computeBoundingCylinder(bcyl);
505 
506  EXPECT_NEAR(0.0, bcyl.pose.translation().x(), 1e-4);
507  EXPECT_NEAR(0.0, bcyl.pose.translation().y(), 1e-4);
508  EXPECT_NEAR(0.0, bcyl.pose.translation().z(), 1e-4);
509  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
510  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
511  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
512  EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
513  radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1);
514  EXPECT_LE(radiusScaled, bcyl.radius);
515  EXPECT_GE(radiusScaled + 1.0, bcyl.radius);
516  lengthScaled = 6.0;
517  EXPECT_LE(lengthScaled, bcyl.length);
518  EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.length);
519 
520  delete m;
521 }
522 
523 TEST(MeshBoundingCylinder, Mesh2)
524 {
525  shapes::Mesh* m = createBoxMesh({ -0.5, -1.0, -1.5 }, { 0.5, 1.0, 1.5 });
526 
527  bodies::ConvexMesh body(m);
528  Eigen::Isometry3d pose;
529  pose.setIdentity();
530  pose.translation() = Eigen::Vector3d(1, 2, 3);
531  body.setPose(pose);
532  bodies::BoundingCylinder bcyl;
533  body.computeBoundingCylinder(bcyl);
534 
535  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
536  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
537  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
538  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
539  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
540  EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
541  EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
542  // for meshes without padding, the bounding cylinder should be tight
543  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
544  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
545 
546  Eigen::AngleAxisd rot(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
547  pose *= rot;
548  body.setPose(pose);
549  body.computeBoundingCylinder(bcyl);
550 
551  EXPECT_NEAR(1.0, bcyl.pose.translation().x(), 1e-4);
552  EXPECT_NEAR(2.0, bcyl.pose.translation().y(), 1e-4);
553  EXPECT_NEAR(3.0, bcyl.pose.translation().z(), 1e-4);
554  EXPECT_NEAR(Eigen::Quaterniond(rot).x(), Eigen::Quaterniond(bcyl.pose.linear()).x(), 1e-4);
555  EXPECT_NEAR(Eigen::Quaterniond(rot).y(), Eigen::Quaterniond(bcyl.pose.linear()).y(), 1e-4);
556  EXPECT_NEAR(Eigen::Quaterniond(rot).z(), Eigen::Quaterniond(bcyl.pose.linear()).z(), 1e-4);
557  EXPECT_NEAR(Eigen::Quaterniond(rot).w(), Eigen::Quaterniond(bcyl.pose.linear()).w(), 1e-4);
558  // for meshes without padding, the bounding cylinder should be tight
559  EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.radius, 1e-4);
560  EXPECT_NEAR(3.0, bcyl.length, 1e-4);
561 
562  delete m;
563 }
564 
565 int main(int argc, char** argv)
566 {
567  testing::InitGoogleTest(&argc, argv);
568  return RUN_ALL_TESTS();
569 }
random_numbers.h
shapes::Mesh::triangles
unsigned int * triangles
The vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1,...
Definition: shapes.h:354
bodies::Sphere
Definition of a sphere.
Definition: bodies.h:327
shapes::Cylinder
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
bodies::BoundingCylinder::radius
double radius
Definition: bodies.h:105
shapes::Mesh
Definition of a triangle mesh By convention the "center" of the shape is at the origin....
Definition: shapes.h:281
bodies::Cylinder::computeBoundingCylinder
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:421
body_operations.h
createBoxMesh
shapes::Mesh * createBoxMesh(const Eigen::Vector3d &min, const Eigen::Vector3d &max)
Definition: test_bounding_cylinder.cpp:319
shapes::Box
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
random_numbers::RandomNumberGenerator::quaternion
void quaternion(double value[4])
shapes::Sphere
Definition of a sphere.
Definition: shapes.h:106
bodies::Box::computeBoundingCylinder
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:648
bodies::BoundingCylinder::length
double length
Definition: bodies.h:106
y
double y
Definition: mesh_operations.cpp:202
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
bodies::BoundingCylinder
Definition of a cylinder.
Definition: bodies.h:102
M_PI_2
#define M_PI_2
bodies::Box
Definition of a box.
Definition: bodies.h:443
bodies::BoundingCylinder::pose
Eigen::Isometry3d pose
Definition: bodies.h:104
shapes::Mesh::vertices
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,...
Definition: shapes.h:347
TEST
TEST(SphereBoundingCylinder, Sphere1)
Definition: test_bounding_cylinder.cpp:44
random_numbers::RandomNumberGenerator
bodies::Sphere::computeBoundingCylinder
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:197
bodies.h
bodies::Body::setScale
void setScale(double scale)
If the dimension of the body should be scaled, this method sets the scale. Default is 1....
Definition: bodies.h:152
bodies::Body::setPose
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:205
x
double x
Definition: mesh_operations.cpp:202
bodies::Body::setPadding
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0....
Definition: bodies.h:179
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
main
int main(int argc, char **argv)
Definition: test_bounding_cylinder.cpp:565
z
double z
Definition: mesh_operations.cpp:202
bodies::ConvexMesh::computeBoundingCylinder
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:1165

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