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