test_distance_field.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
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 Willow Garage 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 /* Author: Mrinal Kalakrishnan, Ken Anderson */
36 
37 #include <gtest/gtest.h>
38 
43 #include <tf2_eigen/tf2_eigen.h>
44 #include <octomap/octomap.h>
45 #include <ros/console.h>
46 
47 #include <memory>
48 
49 using namespace distance_field;
50 
51 static const double width = 1.0;
52 static const double height = 1.0;
53 static const double depth = 1.0;
54 static const double resolution = 0.1;
55 static const double ORIGIN_X = 0.0;
56 static const double ORIGIN_Y = 0.0;
57 static const double ORIGIN_Z = 0.0;
58 static const double max_dist = 0.3;
59 
60 static const Eigen::Vector3d point1(0.1, 0.0, 0.0);
61 static const Eigen::Vector3d POINT2(0.0, 0.1, 0.2);
62 static const Eigen::Vector3d POINT3(0.4, 0.0, 0.0);
63 
64 int dist_sq(int x, int y, int z)
65 {
66  return x * x + y * y + z * z;
67 }
68 
69 void print(PropagationDistanceField& pdf, int numX, int numY, int numZ)
70 {
71  for (int z = 0; z < numZ; z++)
72  {
73  for (int y = 0; y < numY; y++)
74  {
75  for (int x = 0; x < numX; x++)
76  {
77  std::cout << pdf.getCell(x, y, z).distance_square_ << " ";
78  }
79  std::cout << std::endl;
80  }
81  std::cout << std::endl;
82  }
83  for (int z = 0; z < numZ; z++)
84  {
85  for (int y = 0; y < numY; y++)
86  {
87  for (int x = 0; x < numX; x++)
88  {
89  if (pdf.getCell(x, y, z).distance_square_ == 0)
90  {
91  // ROS_INFO_NAMED("distance_field", "Obstacle cell %d %d %d", x, y, z);
92  }
93  }
94  }
95  }
96 }
97 
98 void printNeg(PropagationDistanceField& pdf, int numX, int numY, int numZ)
99 {
100  for (int z = 0; z < numZ; z++)
101  {
102  for (int y = 0; y < numY; y++)
103  {
104  for (int x = 0; x < numX; x++)
105  {
106  std::cout << pdf.getCell(x, y, z).negative_distance_square_ << " ";
107  }
108  std::cout << std::endl;
109  }
110  std::cout << std::endl;
111  }
112 }
113 
114 void printPointCoords(const Eigen::Vector3i& p)
115 {
116  if (p.x() < 0)
117  std::cout << '-';
118  else
119  std::cout << p.x();
120 
121  if (p.y() < 0)
122  std::cout << '-';
123  else
124  std::cout << p.y();
125 
126  if (p.z() < 0)
127  std::cout << '-';
128  else
129  std::cout << p.z();
130 
131  std::cout << " ";
132 }
133 
134 void printBoth(PropagationDistanceField& pdf, int numX, int numY, int numZ)
135 {
136  std::cout << "Positive distance square ... negative distance square" << std::endl;
137  for (int z = 0; z < numZ; z++)
138  {
139  std::cout << "Z=" << z << std::endl;
140  for (int y = 0; y < numY; y++)
141  {
142  for (int x = 0; x < numX; x++)
143  {
144  std::cout << pdf.getCell(x, y, z).distance_square_ << " ";
145  }
146  std::cout << " ";
147  for (int x = 0; x < numX; x++)
148  {
149  std::cout << pdf.getCell(x, y, z).negative_distance_square_ << " ";
150  }
151  std::cout << " ";
152  for (int x = 0; x < numX; x++)
153  {
155  }
156  std::cout << " ";
157  for (int x = 0; x < numX; x++)
158  {
160  }
161  std::cout << std::endl;
162  }
163  std::cout << std::endl;
164  }
165 }
166 
168 {
169  if (df1.getXNumCells() != df2.getXNumCells())
170  return false;
171  if (df1.getYNumCells() != df2.getYNumCells())
172  return false;
173  if (df1.getZNumCells() != df2.getZNumCells())
174  return false;
175  for (int z = 0; z < df1.getZNumCells(); z++)
176  {
177  for (int x = 0; x < df1.getXNumCells(); x++)
178  {
179  for (int y = 0; y < df1.getYNumCells(); y++)
180  {
181  if (df1.getCell(x, y, z).distance_square_ != df2.getCell(x, y, z).distance_square_)
182  {
183  printf("Cell %d %d %d distances not equal %d %d\n", x, y, z, df1.getCell(x, y, z).distance_square_,
184  df2.getCell(x, y, z).distance_square_);
185  return false;
186  }
188  {
189  printf("Cell %d %d %d neg distances not equal %d %d\n", x, y, z,
191  return false;
192  }
193  }
194  }
195  }
196  return true;
197 }
198 
200 {
201  // just one way for now
202  for (int z = 0; z < df.getZNumCells(); z++)
203  {
204  for (int x = 0; x < df.getXNumCells(); x++)
205  {
206  for (int y = 0; y < df.getYNumCells(); y++)
207  {
208  if (df.getCell(x, y, z).distance_square_ == 0)
209  {
210  // making sure that octomap also shows it as occupied
211  double qx, qy, qz;
212  df.gridToWorld(x, y, z, qx, qy, qz);
213  octomap::point3d query(qx, qy, qz);
214  octomap::OcTreeNode* result = octree.search(query);
215  if (!result)
216  {
217  for (float boundary_x = query.x() - df.getResolution();
218  boundary_x <= query.x() + df.getResolution() && !result; boundary_x += df.getResolution())
219  {
220  for (float boundary_y = query.y() - df.getResolution();
221  boundary_y <= query.y() + df.getResolution() && !result; boundary_y += df.getResolution())
222  {
223  for (float boundary_z = query.z() - df.getResolution(); boundary_z <= query.z() + df.getResolution();
224  boundary_z += df.getResolution())
225  {
226  octomap::point3d query_boundary(boundary_x, boundary_y, boundary_z);
227  result = octree.search(query_boundary);
228  if (result)
229  {
230  break;
231  }
232  }
233  }
234  }
235  if (!result)
236  {
237  std::cout << "No point at potential boundary query " << query.x() << " " << query.y() << " " << query.z()
238  << std::endl;
239  return false;
240  }
241  }
242  if (!octree.isNodeOccupied(result))
243  {
244  std::cout << "Disagreement at " << qx << " " << qy << " " << qz << std::endl;
245  return false;
246  }
247  }
248  }
249  }
250  }
251  return true;
252 }
253 
255 {
256  unsigned int count = 0;
257  for (int z = 0; z < df.getZNumCells(); z++)
258  {
259  for (int x = 0; x < df.getXNumCells(); x++)
260  {
261  for (int y = 0; y < df.getYNumCells(); y++)
262  {
263  if (df.getCell(x, y, z).distance_square_ == 0)
264  {
265  count++;
266  }
267  }
268  }
269  }
270  return count;
271 }
272 
273 unsigned int countLeafNodes(const octomap::OcTree& octree)
274 {
275  unsigned int count = 0;
276  for (octomap::OcTree::leaf_iterator it = octree.begin_leafs(), end = octree.end_leafs(); it != end; ++it)
277  {
278  if (octree.isNodeOccupied(*it))
279  {
280  std::cout << "Leaf node " << it.getX() << " " << it.getY() << " " << it.getZ() << std::endl;
281  count++;
282  }
283  }
284  return count;
285 }
286 
287 // points should contain all occupied points
289  int numY, int numZ, bool do_negs)
290 {
291  EigenSTL::vector_Vector3i points_ind(points.size());
292  for (unsigned int i = 0; i < points.size(); i++)
293  {
294  Eigen::Vector3i loc;
295  df.worldToGrid(points[i].x(), points[i].y(), points[i].z(), loc.x(), loc.y(), loc.z());
296  points_ind[i] = loc;
297  }
298 
299  for (int x = 0; x < numX; x++)
300  {
301  for (int y = 0; y < numY; y++)
302  {
303  for (int z = 0; z < numZ; z++)
304  {
305  double dsq = df.getCell(x, y, z).distance_square_;
306  double ndsq = df.getCell(x, y, z).negative_distance_square_;
307  if (dsq == 0)
308  {
309  bool found = false;
310  for (unsigned int i = 0; i < points_ind.size(); i++)
311  {
312  if (points_ind[i].x() == x && points_ind[i].y() == y && points_ind[i].z() == z)
313  {
314  found = true;
315  break;
316  }
317  }
318  if (do_negs)
319  {
320  ASSERT_GT(ndsq, 0) << "Obstacle point " << x << " " << y << " " << z << " has zero negative value";
321  }
322  ASSERT_TRUE(found) << "Obstacle point " << x << " " << y << " " << z << " not found";
323  }
324  }
325  }
326  }
327 }
328 
329 TEST(TestPropagationDistanceField, TestAddRemovePoints)
330 {
332 
333  // Check size
334  int numX = df.getXNumCells();
335  int numY = df.getYNumCells();
336  int numZ = df.getZNumCells();
337 
338  EXPECT_EQ(numX, (int)(width / resolution + 0.5));
339  EXPECT_EQ(numY, (int)(height / resolution + 0.5));
340  EXPECT_EQ(numZ, (int)(depth / resolution + 0.5));
341 
342  // getting a bad point
343  double tgx, tgy, tgz;
344  bool in_bounds;
345  EXPECT_NEAR(df.getDistance(1000.0, 1000.0, 1000.0), max_dist, .0001);
346  EXPECT_NEAR(df.getDistanceGradient(1000.0, 1000.0, 1000.0, tgx, tgy, tgz, in_bounds), max_dist, .0001);
347  EXPECT_FALSE(in_bounds);
348 
349  // Add points to the grid
351  points.push_back(point1);
352  points.push_back(POINT2);
353  ROS_INFO_NAMED("distance_field", "Adding %zu points", points.size());
354  df.addPointsToField(points);
355  // print(df, numX, numY, numZ);
356 
357  // Update - iterative
358  points.clear();
359  points.push_back(POINT2);
360  points.push_back(POINT3);
361  EigenSTL::vector_Vector3d old_points;
362  old_points.push_back(point1);
363  df.updatePointsInField(old_points, points);
364  // std::cout << "One removal, one addition" << std::endl;
365  // print(df, numX, numY, numZ);
366  // printNeg(df, numX, numY, numZ);
367  check_distance_field(df, points, numX, numY, numZ, false);
368 
369  // Remove
370  points.clear();
371  points.push_back(POINT2);
372  df.removePointsFromField(points);
373  points.clear();
374  points.push_back(POINT3);
375  check_distance_field(df, points, numX, numY, numZ, false);
376 
377  // now testing gradient calls
378  df.reset();
379  points.clear();
380  points.push_back(point1);
381  df.addPointsToField(points);
382  bool first = true;
383  for (int z = 1; z < df.getZNumCells() - 1; z++)
384  {
385  for (int x = 1; x < df.getXNumCells() - 1; x++)
386  {
387  for (int y = 1; y < df.getYNumCells() - 1; y++)
388  {
389  double dist = df.getDistance(x, y, z);
390  double wx, wy, wz;
391  df.gridToWorld(x, y, z, wx, wy, wz);
392  Eigen::Vector3d grad(0.0, 0.0, 0.0);
393  bool grad_in_bounds;
394  double dist_grad = df.getDistanceGradient(wx, wy, wz, grad.x(), grad.y(), grad.z(), grad_in_bounds);
395  ASSERT_TRUE(grad_in_bounds) << x << " " << y << " " << z;
396  ASSERT_NEAR(dist, dist_grad, .0001);
397  if (dist > 0 && dist < max_dist)
398  {
399  double xscale = grad.x() / grad.norm();
400  double yscale = grad.y() / grad.norm();
401  double zscale = grad.z() / grad.norm();
402 
403  double comp_x = wx - xscale * dist;
404  double comp_y = wy - yscale * dist;
405  double comp_z = wz - zscale * dist;
406  if (first)
407  {
408  first = false;
409  std::cout << "Dist " << dist << std::endl;
410  std::cout << "Cell " << x << " " << y << " " << z << " " << wx << " " << wy << " " << wz << std::endl;
411  std::cout << "Scale " << xscale << " " << yscale << " " << zscale << std::endl;
412  std::cout << "Grad " << grad.x() << " " << grad.y() << " " << grad.z() << " comp " << comp_x << " "
413  << comp_y << " " << comp_z << std::endl;
414  }
415  ASSERT_NEAR(comp_x, point1.x(), resolution) << dist << x << " " << y << " " << z << " " << grad.x() << " "
416  << grad.y() << " " << grad.z() << " " << xscale << " " << yscale
417  << " " << zscale << std::endl;
418  ASSERT_NEAR(comp_y, point1.y(), resolution) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y()
419  << " " << grad.z() << " " << xscale << " " << yscale << " "
420  << zscale << std::endl;
421  ASSERT_NEAR(comp_z, point1.z(), resolution) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y()
422  << " " << grad.z() << " " << xscale << " " << yscale << " "
423  << zscale << std::endl;
424  }
425  }
426  }
427  }
428  ASSERT_FALSE(first);
429 }
430 
431 TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints)
432 {
434 
435  // Check size
436  int numX = df.getXNumCells();
437  int numY = df.getYNumCells();
438  int numZ = df.getZNumCells();
439 
440  EXPECT_EQ(numX, (int)(width / resolution + 0.5));
441  EXPECT_EQ(numY, (int)(height / resolution + 0.5));
442  EXPECT_EQ(numZ, (int)(depth / resolution + 0.5));
443 
444  // Error checking
445  // print(df, numX, numY, numZ);
446 
447  // TODO - check initial values
448  // EXPECT_EQ( df.getCell(0,0,0).distance_square_, max_dist_sq_in_voxels );
449 
450  // Add points to the grid
451  double lwx, lwy, lwz;
452  double hwx, hwy, hwz;
453  df.gridToWorld(1, 1, 1, lwx, lwy, lwz);
454  df.gridToWorld(8, 8, 8, hwx, hwy, hwz);
455 
457  for (double x = lwx; x <= hwx; x += .1)
458  {
459  for (double y = lwy; y <= hwy; y += .1)
460  {
461  for (double z = lwz; z <= hwz; z += .1)
462  {
463  points.push_back(Eigen::Vector3d(x, y, z));
464  }
465  }
466  }
467 
468  df.reset();
469  ROS_INFO_NAMED("distance_field", "Adding %zu points", points.size());
470  df.addPointsToField(points);
471  // print(df, numX, numY, numZ);
472  // printNeg(df, numX, numY, numZ);
473 
474  double cx, cy, cz;
475  df.gridToWorld(5, 5, 5, cx, cy, cz);
476 
477  Eigen::Vector3d center_point(cx, cy, cz);
478 
479  EigenSTL::vector_Vector3d rem_points;
480  rem_points.push_back(center_point);
481  df.removePointsFromField(rem_points);
482  // std::cout << "Pos "<< std::endl;
483  // print(df, numX, numY, numZ);
484  // std::cout << "Neg "<< std::endl;
485  // printNeg(df, numX, numY, numZ);
486 
487  // testing equality with initial add of points without the center point
489  EigenSTL::vector_Vector3d test_points;
490  for (unsigned int i = 0; i < points.size(); i++)
491  {
492  if (points[i].x() != center_point.x() || points[i].y() != center_point.y() || points[i].z() != center_point.z())
493  {
494  test_points.push_back(points[i]);
495  }
496  }
497  test_df.addPointsToField(test_points);
498  ASSERT_TRUE(areDistanceFieldsDistancesEqual(df, test_df));
499 
501 
502  shapes::Sphere sphere(.25);
503 
504  geometry_msgs::Pose p;
505  p.orientation.w = 1.0;
506  p.position.x = .5;
507  p.position.y = .5;
508  p.position.z = .5;
509 
510  Eigen::Isometry3d p_eigen;
511  tf2::fromMsg(p, p_eigen);
512 
513  gradient_df.addShapeToField(&sphere, p_eigen);
514  // printBoth(gradient_df, numX, numY, numZ);
515  EXPECT_GT(gradient_df.getCell(5, 5, 5).negative_distance_square_, 1);
516  // all negative cells should have gradients that point towards cells with distance 1
517  for (int z = 1; z < df.getZNumCells() - 1; z++)
518  {
519  for (int x = 1; x < df.getXNumCells() - 1; x++)
520  {
521  for (int y = 1; y < df.getYNumCells() - 1; y++)
522  {
523  double dist = gradient_df.getDistance(x, y, z);
524  double ncell_dist;
525  Eigen::Vector3i ncell_pos;
526  const PropDistanceFieldVoxel* ncell = gradient_df.getNearestCell(x, y, z, ncell_dist, ncell_pos);
527 
528  EXPECT_EQ(ncell_dist, dist);
529 
530  if (ncell == nullptr)
531  {
532  if (ncell_dist > 0)
533  {
534  EXPECT_GE(ncell_dist, gradient_df.getUninitializedDistance())
535  << "dist=" << dist << " xyz=" << x << " " << y << " " << z << " ncell=" << ncell_pos.x() << " "
536  << ncell_pos.y() << " " << ncell_pos.z() << std::endl;
537  }
538  else if (ncell_dist < 0)
539  {
540  EXPECT_LE(ncell_dist, -gradient_df.getUninitializedDistance())
541  << "dist=" << dist << " xyz=" << x << " " << y << " " << z << " ncell=" << ncell_pos.x() << " "
542  << ncell_pos.y() << " " << ncell_pos.z() << std::endl;
543  }
544  }
545 
546  if (gradient_df.getCell(x, y, z).negative_distance_square_ > 0)
547  {
548  ASSERT_LT(dist, 0) << "Pos " << gradient_df.getCell(x, y, z).distance_square_ << " "
549  << gradient_df.getCell(x, y, z).negative_distance_square_;
550  double wx, wy, wz;
551  df.gridToWorld(x, y, z, wx, wy, wz);
552  Eigen::Vector3d grad(0.0, 0.0, 0.0);
553  bool grad_in_bounds;
554  double dist_grad = gradient_df.getDistanceGradient(wx, wy, wz, grad.x(), grad.y(), grad.z(), grad_in_bounds);
555  ASSERT_TRUE(grad_in_bounds) << x << " " << y << " " << z;
556  ASSERT_NEAR(dist, dist_grad, .0001);
557 
558  if (!ncell)
559  continue;
560 
561  EXPECT_GE(gradient_df.getCell(ncell_pos.x(), ncell_pos.y(), ncell_pos.z()).distance_square_, 1)
562  << "dist=" << dist << " xyz=" << x << " " << y << " " << z << " grad=" << grad.x() << " " << grad.y()
563  << " " << grad.z() << " ncell=" << ncell_pos.x() << " " << ncell_pos.y() << " " << ncell_pos.z()
564  << std::endl;
565 
566  double grad_size_sq = grad.squaredNorm();
567  if (grad_size_sq < std::numeric_limits<double>::epsilon())
568  continue;
569 
570  double oo_grad_size = 1.0 / sqrt(grad_size_sq);
571  double xscale = grad.x() * oo_grad_size;
572  double yscale = grad.y() * oo_grad_size;
573  double zscale = grad.z() * oo_grad_size;
574 
575  double comp_x = wx - xscale * dist;
576  double comp_y = wy - yscale * dist;
577  double comp_z = wz - zscale * dist;
578 
579  int cell_x, cell_y, cell_z;
580  bool cell_in_bounds = gradient_df.worldToGrid(comp_x, comp_y, comp_z, cell_x, cell_y, cell_z);
581 
582  ASSERT_EQ(cell_in_bounds, true);
583  const PropDistanceFieldVoxel* cell = &gradient_df.getCell(cell_x, cell_y, cell_z);
584 
585 #if 0
586  EXPECT_EQ(ncell_pos.x(), cell_x);
587  EXPECT_EQ(ncell_pos.y(), cell_y);
588  EXPECT_EQ(ncell_pos.z(), cell_z);
589  EXPECT_EQ(ncell, cell);
590 #endif
591  EXPECT_GE(cell->distance_square_, 1) << dist << " " << x << " " << y << " " << z << " " << grad.x() << " "
592  << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " "
593  << zscale << " cell " << comp_x << " " << comp_y << " " << comp_z
594  << std::endl;
595  }
596  }
597  }
598  }
599 }
600 
601 TEST(TestSignedPropagationDistanceField, TestShape)
602 {
604 
605  int num_x = df.getXNumCells();
606  int num_y = df.getYNumCells();
607  int num_z = df.getZNumCells();
608 
609  shapes::Sphere sphere(.25);
610 
611  Eigen::Isometry3d p = Eigen::Translation3d(0.5, 0.5, 0.5) * Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
612  Eigen::Isometry3d np = Eigen::Translation3d(0.7, 0.7, 0.7) * Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
613 
614  df.addShapeToField(&sphere, p);
615 
616  bodies::Body* body = bodies::createBodyFromShape(&sphere);
617  body->setPose(p);
618  EigenSTL::vector_Vector3d point_vec;
619  findInternalPointsConvex(*body, resolution, point_vec);
620  delete body;
621  check_distance_field(df, point_vec, num_x, num_y, num_z, true);
622 
623  // std::cout << "Shape pos "<< std::endl;
624  // print(df, numX, numY, numZ);
625  // std::cout << "Shape neg "<< std::endl;
626  // printNeg(df, numX, numY, numZ);
627 
628  df.addShapeToField(&sphere, np);
629 
630  body = bodies::createBodyFromShape(&sphere);
631  body->setPose(np);
632 
633  EigenSTL::vector_Vector3d point_vec_2;
634  findInternalPointsConvex(*body, resolution, point_vec_2);
635  delete body;
636  EigenSTL::vector_Vector3d point_vec_union = point_vec_2;
637  point_vec_union.insert(point_vec_union.end(), point_vec.begin(), point_vec.end());
638  check_distance_field(df, point_vec_union, num_x, num_y, num_z, true);
639 
640  // should get rid of old pose
641  df.moveShapeInField(&sphere, p, np);
642 
643  check_distance_field(df, point_vec_2, num_x, num_y, num_z, true);
644 
645  // should be equivalent to just adding second shape
647  test_df.addShapeToField(&sphere, np);
648  ASSERT_TRUE(areDistanceFieldsDistancesEqual(df, test_df));
649 }
650 
651 static const double PERF_WIDTH = 3.0;
652 static const double PERF_HEIGHT = 3.0;
653 static const double PERF_DEPTH = 4.0;
654 static const double PERF_RESOLUTION = 0.02;
655 static const double PERF_ORIGIN_X = 0.0;
656 static const double PERF_ORIGIN_Y = 0.0;
657 static const double PERF_ORIGIN_Z = 0.0;
658 static const double PERF_MAX_DIST = .25;
659 static const unsigned int UNIFORM_DISTANCE = 10;
660 
661 TEST(TestSignedPropagationDistanceField, TestPerformance)
662 {
663  std::cout << "Creating distance field with "
665  << " entries" << std::endl;
666 
669  PERF_ORIGIN_Z, PERF_MAX_DIST, false);
670  std::cout << "Creating unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl;
671 
672  dt = ros::WallTime::now();
675 
676  std::cout << "Creating signed took " << (ros::WallTime::now() - dt).toSec() << std::endl;
677 
678  shapes::Box big_table(2.0, 2.0, .5);
679 
680  Eigen::Isometry3d p = Eigen::Translation3d(PERF_WIDTH / 2.0, PERF_DEPTH / 2.0, PERF_HEIGHT / 2.0) *
681  Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
682  Eigen::Isometry3d np = Eigen::Translation3d(PERF_WIDTH / 2.0 + .01, PERF_DEPTH / 2.0, PERF_HEIGHT / 2.0) *
683  Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
684 
685  unsigned int big_num_points = ceil(2.0 / PERF_RESOLUTION) * ceil(2.0 / PERF_RESOLUTION) * ceil(.5 / PERF_RESOLUTION);
686 
687  std::cout << "Adding " << big_num_points << " points" << std::endl;
688 
689  dt = ros::WallTime::now();
690  df.addShapeToField(&big_table, p);
691  std::cout << "Adding to unsigned took " << (ros::WallTime::now() - dt).toSec() << " avg "
692  << (ros::WallTime::now() - dt).toSec() / (big_num_points * 1.0) << std::endl;
693 
694  dt = ros::WallTime::now();
695  df.addShapeToField(&big_table, p);
696  std::cout << "Re-adding to unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl;
697 
698  dt = ros::WallTime::now();
699  sdf.addShapeToField(&big_table, p);
700  std::cout << "Adding to signed took " << (ros::WallTime::now() - dt).toSec() << " avg "
701  << (ros::WallTime::now() - dt).toSec() / (big_num_points * 1.0) << std::endl;
702 
703  dt = ros::WallTime::now();
704  df.moveShapeInField(&big_table, p, np);
705  std::cout << "Moving in unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl;
706 
707  dt = ros::WallTime::now();
708  sdf.moveShapeInField(&big_table, p, np);
709  std::cout << "Moving in signed took " << (ros::WallTime::now() - dt).toSec() << std::endl;
710 
711  dt = ros::WallTime::now();
712  df.removeShapeFromField(&big_table, np);
713  std::cout << "Removing from unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl;
714 
715  dt = ros::WallTime::now();
716  sdf.removeShapeFromField(&big_table, np);
717  std::cout << "Removing from signed took " << (ros::WallTime::now() - dt).toSec() << std::endl;
718 
719  dt = ros::WallTime::now();
720  df.reset();
721 
722  shapes::Box small_table(.25, .25, .05);
723 
724  unsigned int small_num_points = (13) * (13) * (3);
725 
726  std::cout << "Adding " << small_num_points << " points" << std::endl;
727 
728  dt = ros::WallTime::now();
729  df.addShapeToField(&small_table, p);
730  std::cout << "Adding to unsigned took " << (ros::WallTime::now() - dt).toSec() << " avg "
731  << (ros::WallTime::now() - dt).toSec() / (small_num_points * 1.0) << std::endl;
732 
733  dt = ros::WallTime::now();
734  sdf.addShapeToField(&small_table, p);
735  std::cout << "Adding to signed took " << (ros::WallTime::now() - dt).toSec() << " avg "
736  << (ros::WallTime::now() - dt).toSec() / (small_num_points * 1.0) << std::endl;
737 
738  dt = ros::WallTime::now();
739  df.moveShapeInField(&small_table, p, np);
740  std::cout << "Moving in unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl;
741 
742  dt = ros::WallTime::now();
743  sdf.moveShapeInField(&small_table, p, np);
744  std::cout << "Moving in signed took " << (ros::WallTime::now() - dt).toSec() << std::endl;
745 
746  // uniformly spaced points - a worst case scenario
748  PERF_ORIGIN_Z, PERF_MAX_DIST, false);
749 
752 
754  unsigned int count = 0;
755  for (unsigned int z = UNIFORM_DISTANCE; z < worstdfu.getZNumCells() - UNIFORM_DISTANCE; z += UNIFORM_DISTANCE)
756  {
757  for (unsigned int x = UNIFORM_DISTANCE; x < worstdfu.getXNumCells() - UNIFORM_DISTANCE; x += UNIFORM_DISTANCE)
758  {
759  for (unsigned int y = UNIFORM_DISTANCE; y < worstdfu.getYNumCells() - UNIFORM_DISTANCE; y += UNIFORM_DISTANCE)
760  {
761  count++;
762  Eigen::Vector3d loc;
763  bool valid = worstdfu.gridToWorld(x, y, z, loc.x(), loc.y(), loc.z());
764 
765  if (!valid)
766  {
767  ROS_WARN_NAMED("distance_field", "Something wrong");
768  continue;
769  }
770  bad_vec.push_back(loc);
771  }
772  }
773  }
774 
775  dt = ros::WallTime::now();
776  worstdfu.addPointsToField(bad_vec);
778  printf("Time for unsigned adding %u uniform points is %g average %g\n", (unsigned int)bad_vec.size(), wd.toSec(),
779  wd.toSec() / (bad_vec.size() * 1.0));
780  dt = ros::WallTime::now();
781  worstdfs.addPointsToField(bad_vec);
782  wd = ros::WallTime::now() - dt;
783  printf("Time for signed adding %u uniform points is %g average %g\n", (unsigned int)bad_vec.size(), wd.toSec(),
784  wd.toSec() / (bad_vec.size() * 1.0));
785 }
786 
787 TEST(TestSignedPropagationDistanceField, TestOcTree)
788 {
790  PERF_ORIGIN_Z, PERF_MAX_DIST, false);
791 
792  octomap::OcTree tree(.02);
793 
794  unsigned int count = 0;
795  for (float x = 1.01; x < 1.5; x += .02)
796  {
797  for (float y = 1.01; y < 1.5; y += .02)
798  {
799  for (float z = 1.01; z < 1.5; z += .02, count++)
800  {
801  octomap::point3d point(x, y, z);
802  tree.updateNode(point, true);
803  }
804  }
805  }
806 
807  // more points at the border of the distance field
808  for (float x = 2.51; x < 3.5; x += .02)
809  {
810  for (float y = 1.01; y < 3.5; y += .02)
811  {
812  for (float z = 1.01; z < 3.5; z += .02, count++)
813  {
814  octomap::point3d point(x, y, z);
815  tree.updateNode(point, true);
816  }
817  }
818  }
819 
820  std::cout << "OcTree nodes " << count << std::endl;
821  df.addOcTreeToField(&tree);
822 
824 
825  // more cells
826  for (float x = .01; x < .50; x += .02)
827  {
828  for (float y = .01; y < .50; y += .02)
829  {
830  for (float z = .01; z < .50; z += .02, count++)
831  {
832  octomap::point3d point(x, y, z);
833  tree.updateNode(point, true);
834  }
835  }
836  }
837  df.addOcTreeToField(&tree);
839 
840  PropagationDistanceField df_oct(tree, octomap::point3d(0.5, 0.5, 0.5), octomap::point3d(5.0, 5.0, 5.0), PERF_MAX_DIST,
841  false);
842 
844 
845  // now try different resolutions
846  octomap::OcTree tree_lowres(.05);
847  octomap::point3d point1(.5, .5, .5);
848  octomap::point3d point2(.7, .5, .5);
849  octomap::point3d point3(1.0, .5, .5);
850  tree_lowres.updateNode(point1, true);
851  tree_lowres.updateNode(point2, true);
852  tree_lowres.updateNode(point3, true);
853  ASSERT_EQ(countLeafNodes(tree_lowres), 3u);
854 
857 
858  df_highres.addOcTreeToField(&tree_lowres);
859  EXPECT_EQ(countOccupiedCells(df_highres), 3u * (4u * 4u * 4u));
860  std::cout << "Occupied cells " << countOccupiedCells(df_highres) << std::endl;
861 
862  // testing adding shape that happens to be octree
863  std::shared_ptr<octomap::OcTree> tree_shape(new octomap::OcTree(.05));
864  octomap::point3d tpoint1(1.0, .5, 1.0);
865  octomap::point3d tpoint2(1.7, .5, .5);
866  octomap::point3d tpoint3(1.8, .5, .5);
867  tree_shape->updateNode(tpoint1, true);
868  tree_shape->updateNode(tpoint2, true);
869  tree_shape->updateNode(tpoint3, true);
870 
871  std::shared_ptr<shapes::OcTree> shape_oc(new shapes::OcTree(tree_shape));
872 
875 
878 
879  df_test_shape_1.addOcTreeToField(tree_shape.get());
880  df_test_shape_2.addShapeToField(shape_oc.get(), Eigen::Isometry3d());
881  EXPECT_TRUE(areDistanceFieldsDistancesEqual(df_test_shape_1, df_test_shape_2));
882 }
883 
884 TEST(TestSignedPropagationDistanceField, TestReadWrite)
885 {
887 
889  points.push_back(point1);
890  points.push_back(POINT2);
891  points.push_back(POINT3);
892  small_df.addPointsToField(points);
893 
894  std::ofstream sf("test_small.df", std::ios::out);
895 
896  small_df.writeToStream(sf);
897  // must close to make sure that the buffer is written
898  sf.close();
899 
900  std::ifstream si("test_small.df", std::ios::in | std::ios::binary);
901  PropagationDistanceField small_df2(si, PERF_MAX_DIST, false);
902  ASSERT_TRUE(areDistanceFieldsDistancesEqual(small_df, small_df2));
903 
905  PERF_ORIGIN_Z, PERF_MAX_DIST, false);
906 
907  shapes::Sphere sphere(.5);
908 
909  Eigen::Isometry3d p = Eigen::Translation3d(0.5, 0.5, 0.5) * Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
910 
911  df.addShapeToField(&sphere, p);
912 
913  std::ofstream f("test_big.df", std::ios::out);
914 
915  df.writeToStream(f);
916  f.close();
917 
918  std::ifstream i("test_big.df", std::ios::in);
921 
922  // we shouldn't segfault if we start with an old ifstream
924 
925  std::ifstream i2("test_big.df", std::ios::in);
927  PropagationDistanceField df3(i2, PERF_MAX_DIST + .02, false);
928  std::cout << "Reconstruction for big file took " << (ros::WallTime::now() - wt).toSec() << std::endl;
930 }
931 
932 int main(int argc, char** argv)
933 {
934  testing::InitGoogleTest(&argc, argv);
935  return RUN_ALL_TESTS();
936 }
bool areDistanceFieldsDistancesEqual(const PropagationDistanceField &df1, const PropagationDistanceField &df2)
static const Eigen::Vector3d POINT3(0.4, 0.0, 0.0)
void addShapeToField(const shapes::Shape *shape, const Eigen::Isometry3d &pose)
Adds the set of points corresponding to the shape at the given pose as obstacle points into the dista...
#define ROS_INFO_NAMED(name,...)
void check_distance_field(const PropagationDistanceField &df, const EigenSTL::vector_Vector3d &points, int numX, int numY, int numZ, bool do_negs)
void setPose(const Eigen::Isometry3d &pose)
int getZNumCells() const override
Gets the number of cells along the Z axis.
unsigned int countLeafNodes(const octomap::OcTree &octree)
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
#define ROS_WARN_NAMED(name,...)
bool writeToStream(std::ostream &stream) const override
Writes the contents of the distance field to the supplied stream.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
f
static const double ORIGIN_Z
const PropDistanceFieldVoxel & getCell(int x, int y, int z) const
Gets full cell data given an index.
int distance_square_
Distance in cells to the closest obstacle, squared.
static const double width
static const unsigned int UNIFORM_DISTANCE
virtual OcTreeNode * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
Structure that holds voxel information for the DistanceField. Will be used in VoxelGrid.
bool gridToWorld(int x, int y, int z, double &world_x, double &world_y, double &world_z) const override
Converts from an set of integer indices to a world location given the origin and resolution parameter...
void addPointsToField(const EigenSTL::vector_Vector3d &points) override
Add a set of obstacle points to the distance field, updating distance values accordingly. The distance field may already contain obstacle cells.
#define EXPECT_NEAR(a, b, prec)
const PropDistanceFieldVoxel * getNearestCell(int x, int y, int z, double &dist, Eigen::Vector3i &pos) const
Gets nearest surface cell and returns distance to it.
Eigen::Vector3i closest_point_
Closest occupied cell.
Eigen::Vector3i closest_negative_point_
Closest unoccupied cell.
int getXNumCells() const override
Gets the number of cells along the X axis.
double y
void moveShapeInField(const shapes::Shape *shape, const Eigen::Isometry3d &old_pose, const Eigen::Isometry3d &new_pose)
Moves the shape in the distance field from the old pose to the new pose, removing points that are no ...
static const double PERF_WIDTH
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
#define EXPECT_FALSE(args)
static const double PERF_DEPTH
#define EXPECT_EQ(a, b)
OcTreeNode * search(double x, double y, double z, unsigned int depth=0) const
static const double resolution
static const double PERF_HEIGHT
void reset() override
Resets the entire distance field to max_distance for positive values and zero for negative values...
static const double ORIGIN_X
double getDistanceGradient(double x, double y, double z, double &gradient_x, double &gradient_y, double &gradient_z, bool &in_bounds) const
Gets not only the distance to the nearest cell but the gradient direction. The gradient is computed a...
void printBoth(PropagationDistanceField &pdf, int numX, int numY, int numZ)
static const double PERF_ORIGIN_Y
Body * createBodyFromShape(const shapes::Shape *shape)
bool worldToGrid(double world_x, double world_y, double world_z, int &x, int &y, int &z) const override
Converts from a world location to a set of integer indices. Should return false if the world location...
void fromMsg(const A &, B &b)
int main(int argc, char **argv)
TEST(TestPropagationDistanceField, TestAddRemovePoints)
double z
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > vector_Vector3i
int getYNumCells() const override
Gets the number of cells along the Y axis.
unsigned int countOccupiedCells(const PropagationDistanceField &df)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
static const double depth
double getUninitializedDistance() const override
Gets a distance value for an invalid cell.
static const double PERF_ORIGIN_X
double getResolution() const
Gets the resolution of the distance field in meters.
Namespace for holding classes that generate distance fields.
int dist_sq(int x, int y, int z)
static const Eigen::Vector3d point1(0.1, 0.0, 0.0)
void updatePointsInField(const EigenSTL::vector_Vector3d &old_points, const EigenSTL::vector_Vector3d &new_points) override
This function will remove any obstacle points that are in the old point set but not the new point set...
static const double height
static WallTime now()
static const Eigen::Vector3d POINT2(0.0, 0.1, 0.2)
double getDistance(double x, double y, double z) const override
Get the distance value associated with the cell indicated by the world coordinate. If the cell is invalid, max_distance will be returned. If running without negative distances, all obstacle cells will have zero distance. If running with negative distances, the distance will be between -max_distance and max_distance, with no values having a 0 distance.
bool checkOctomapVersusDistanceField(const PropagationDistanceField &df, const octomap::OcTree &octree)
void removePointsFromField(const EigenSTL::vector_Vector3d &points) override
Remove a set of obstacle points from the distance field, updating distance values accordingly...
static const double PERF_ORIGIN_Z
int negative_distance_square_
Distance in cells to the nearest unoccupied cell, squared.
void removeShapeFromField(const shapes::Shape *shape, const Eigen::Isometry3d &pose)
All points corresponding to the shape are removed from the distance field.
void findInternalPointsConvex(const bodies::Body &body, double resolution, EigenSTL::vector_Vector3d &points)
Find all points on a regular grid that are internal to the body, assuming the body is a convex shape...
static const double max_dist
void addOcTreeToField(const octomap::OcTree *octree)
Adds an octree to the distance field. Cells that are occupied in the octree that lie within the voxel...
A DistanceField implementation that uses a vector propagation method. Distances propagate outward fro...
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
void printNeg(PropagationDistanceField &pdf, int numX, int numY, int numZ)
double x
void printPointCoords(const Eigen::Vector3i &p)
#define EXPECT_TRUE(args)
static const double PERF_MAX_DIST
static const double ORIGIN_Y
static const double PERF_RESOLUTION
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri May 17 2019 02:51:53