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 
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::Affine3d p_eigen;
511  tf::poseMsgToEigen(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 numX = df.getXNumCells();
606  int numY = df.getYNumCells();
607  int numZ = df.getZNumCells();
608 
609  shapes::Sphere sphere(.25);
610 
611  Eigen::Affine3d p = Eigen::Translation3d(0.5, 0.5, 0.5) * Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
612  Eigen::Affine3d 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, numX, numY, numZ, 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, numX, numY, numZ, true);
639 
640  // should get rid of old pose
641  df.moveShapeInField(&sphere, p, np);
642 
643  check_distance_field(df, point_vec_2, numX, numY, numZ, 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::Affine3d 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::Affine3d 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), 3);
854 
857 
858  df_highres.addOcTreeToField(&tree_lowres);
859  EXPECT_EQ(countOccupiedCells(df_highres), 3 * (4 * 4 * 4));
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::Affine3d());
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::Affine3d 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)
virtual void reset()
Resets the entire distance field to max_distance for positive values and zero for negative values...
void setPose(const Eigen::Affine3d &pose)
const PropDistanceFieldVoxel & getCell(int x, int y, int z) const
Gets full cell data given an index.
static const Eigen::Vector3d point2(0.0, 0.1, 0.2)
virtual int getXNumCells() const
Gets the number of cells along the X axis.
static const Eigen::Vector3d point3(0.4, 0.0, 0.0)
#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)
virtual void updatePointsInField(const EigenSTL::vector_Vector3d &old_points, const EigenSTL::vector_Vector3d &new_points)
This function will remove any obstacle points that are in the old point set but not the new point set...
unsigned int countLeafNodes(const octomap::OcTree &octree)
#define ROS_WARN_NAMED(name,...)
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
f
int distance_square_
Distance in cells to the closest obstacle, squared.
static const double width
virtual bool writeToStream(std::ostream &stream) const
Writes the contents of the distance field to the supplied stream.
static const unsigned int UNIFORM_DISTANCE
virtual OcTreeNode * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
Structure that holds voxel information for the DistanceField. Will be used in VoxelGrid.
static const double origin_y
void removeShapeFromField(const shapes::Shape *shape, const Eigen::Affine3d &pose)
All points corresponding to the shape are removed from the distance field.
OcTreeNode * search(double x, double y, double z, unsigned int depth=0) const
double getResolution() const
Gets the resolution of the distance field in meters.
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...
#define EXPECT_NEAR(a, b, prec)
Eigen::Vector3i closest_point_
Closest occupied cell.
Eigen::Vector3i closest_negative_point_
Closest unoccupied cell.
double y
static const double PERF_WIDTH
#define EXPECT_FALSE(args)
static const double PERF_DEPTH
#define EXPECT_EQ(a, b)
virtual bool worldToGrid(double world_x, double world_y, double world_z, int &x, int &y, int &z) const
Converts from a world location to a set of integer indices. Should return false if the world location...
static const double resolution
static const double PERF_HEIGHT
virtual int getYNumCells() const
Gets the number of cells along the Y axis.
void printBoth(PropagationDistanceField &pdf, int numX, int numY, int numZ)
static const double PERF_ORIGIN_Y
static const double origin_z
Body * createBodyFromShape(const shapes::Shape *shape)
int main(int argc, char **argv)
TEST(TestPropagationDistanceField, TestAddRemovePoints)
double z
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > vector_Vector3i
unsigned int countOccupiedCells(const PropagationDistanceField &df)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
static const double depth
virtual void removePointsFromField(const EigenSTL::vector_Vector3d &points)
Remove a set of obstacle points from the distance field, updating distance values accordingly...
static const double PERF_ORIGIN_X
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)
virtual int getZNumCells() const
Gets the number of cells along the Z axis.
virtual double getDistance(double x, double y, double z) const
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.
static const double height
static WallTime now()
bool checkOctomapVersusDistanceField(const PropagationDistanceField &df, const octomap::OcTree &octree)
static const double PERF_ORIGIN_Z
int negative_distance_square_
Distance in cells to the nearest unoccupied cell, squared.
virtual void addPointsToField(const EigenSTL::vector_Vector3d &points)
Add a set of obstacle points to the distance field, updating distance values accordingly. The distance field may already contain obstacle cells.
virtual double getUninitializedDistance() const
Gets a distance value for an invalid cell.
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
const PropDistanceFieldVoxel * getNearestCell(int x, int y, int z, double &dist, Eigen::Vector3i &pos) const
Gets nearest surface cell and returns distance to it.
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...
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
void addShapeToField(const shapes::Shape *shape, const Eigen::Affine3d &pose)
Adds the set of points corresponding to the shape at the given pose as obstacle points into the dista...
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
static const double PERF_RESOLUTION
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)
virtual bool gridToWorld(int x, int y, int z, double &world_x, double &world_y, double &world_z) const
Converts from an set of integer indices to a world location given the origin and resolution parameter...
void moveShapeInField(const shapes::Shape *shape, const Eigen::Affine3d &old_pose, const Eigen::Affine3d &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 origin_x


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33