test_raycast_dda.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2020, the mcl_3dl authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <cmath>
31 #include <cstddef>
32 #include <vector>
33 
34 #include <gtest/gtest.h>
35 
36 #include <mcl_3dl/chunked_kdtree.h>
37 #include <mcl_3dl/point_types.h>
39 
40 TEST(RaycastUsingDDA, Collision)
41 {
42  pcl::PointCloud<pcl::PointXYZ> pc;
43  for (float y = -1.0; y < 1.0; y += 0.1)
44  {
45  for (float z = -1.0; z < 1.0; z += 0.1)
46  {
47  pc.push_back(pcl::PointXYZ(0.5, y, z));
48  }
49  }
50  // Add dummy points to get edges of map
51  pc.push_back(pcl::PointXYZ(-2.05, -2.05, -2.05));
52  pc.push_back(pcl::PointXYZ(2.05, 2.05, 2.05));
53 
55  kdtree->setInputCloud(pc.makeShared());
56  const float epsilon = 0.05;
57  const float hit_range = std::sqrt(3.0) * 0.1;
58  mcl_3dl::RaycastUsingDDA<pcl::PointXYZ> raycaster(0.1, 0.1, 0.1, 0.1, 0.5, hit_range);
59  for (float y = -0.8; y < 0.8; y += 0.11)
60  {
61  for (float z = -0.8; z < 0.8; z += 0.13)
62  {
63  bool collision = false;
64  raycaster.setRay(kdtree, mcl_3dl::Vec3(0.0, 0.0, 0.0), mcl_3dl::Vec3(1.0, y * 2.0, z * 2.0));
66  while (raycaster.getNextCastResult(point))
67  {
68  if (point.collision_)
69  {
70  collision = true;
71  EXPECT_NEAR((point.pos_ - mcl_3dl::Vec3(0.5, y, z)).norm(), 0.0, 0.2);
72  break;
73  }
74  }
75  ASSERT_TRUE(collision);
76  }
77  }
78  for (float y = -1.0; y < 1.0; y += 0.11)
79  {
80  for (float z = -1.0; z < 1.0; z += 0.13)
81  {
82  bool collision = false;
83  raycaster.setRay(kdtree, mcl_3dl::Vec3(0.0, 0.0, 0.0), mcl_3dl::Vec3(0.5 - hit_range - epsilon, y, z));
85  while (raycaster.getNextCastResult(point))
86  {
87  if (point.collision_)
88  collision = true;
89  }
90  ASSERT_FALSE(collision);
91  }
92  }
93  {
94  bool collision = false;
95  raycaster.setRay(kdtree, mcl_3dl::Vec3(0.0, 0.0, 0.0), mcl_3dl::Vec3(0.5, 3.0, 0.0));
97  while (raycaster.getNextCastResult(point))
98  {
99  if (point.collision_)
100  collision = true;
101  }
102  ASSERT_FALSE(collision);
103  }
104 }
105 
106 TEST(RaycastUsingDDA, CollisionTolerance)
107 {
108  pcl::PointCloud<pcl::PointXYZ> pc;
109  for (float y = -1.0; y < 1.0; y += 0.05)
110  {
111  for (float z = -1.0; z < 1.0; z += 0.1)
112  {
113  pc.push_back(pcl::PointXYZ(0.5, y, z));
114  }
115  }
116  // Add dummy points to get edges of map
117  pc.push_back(pcl::PointXYZ(-2.05, -2.05, -2.05));
118  pc.push_back(pcl::PointXYZ(2.05, 2.05, 2.05));
119 
121  kdtree->setInputCloud(pc.makeShared());
122 
123  {
124  mcl_3dl::RaycastUsingDDA<pcl::PointXYZ> raycaster(0.05, 0.1, 0.1, 0.1, 0.5, sqrt(3.0) * 0.1);
125 
126  bool collision = false;
127  raycaster.setRay(kdtree, mcl_3dl::Vec3(0.0, 0.0, 0.0), mcl_3dl::Vec3(0.5, 0.0, 0.0));
129  while (raycaster.getNextCastResult(point))
130  {
131  if (point.collision_)
132  {
133  collision = true;
134  break;
135  }
136  }
137  ASSERT_TRUE(collision);
138  }
139  {
140  bool collision = false;
141  const float hit_range = std::sqrt(3.0) * 0.15;
142  mcl_3dl::RaycastUsingDDA<pcl::PointXYZ> raycaster(0.1, 0.15, 0.15, 0.15, 0.5, hit_range);
143  raycaster.setRay(kdtree, mcl_3dl::Vec3(0.0, 0.0, 0.0), mcl_3dl::Vec3(0.5 - hit_range, 0.0, 0.0));
145  while (raycaster.getNextCastResult(point))
146  {
147  if (point.collision_)
148  {
149  collision = true;
150  break;
151  }
152  }
153  ASSERT_FALSE(collision);
154  }
155 }
156 
158  mcl_3dl::RaycastUsingDDA<pcl::PointXYZ>& raycaster, const mcl_3dl::Vec3& ray_begin,
159  const mcl_3dl::Vec3& ray_end, const std::vector<mcl_3dl::Vec3> expected_points,
160  const bool expected_collision)
161 {
162  raycaster.setRay(kdtree, ray_begin, ray_end);
163  std::vector<mcl_3dl::Vec3> actual_points;
165  bool collision = false;
166  while (raycaster.getNextCastResult(pos))
167  {
168  actual_points.push_back(pos.pos_);
169  if (pos.collision_)
170  {
171  collision = true;
172  break;
173  }
174  }
175  EXPECT_EQ(expected_collision, collision) << "case: " << name;
176  ASSERT_EQ(expected_points.size(), actual_points.size()) << "case: " << name;
177  for (size_t i = 0; i < expected_points.size(); ++i)
178  {
179  EXPECT_NEAR(expected_points[i].x_, actual_points[i].x_, 1.0e-6f) << "case: " << name;
180  EXPECT_NEAR(expected_points[i].y_, actual_points[i].y_, 1.0e-6f) << "case: " << name;
181  EXPECT_NEAR(expected_points[i].z_, actual_points[i].z_, 1.0e-6f) << "case: " << name;
182  }
183 }
184 
185 TEST(RaycastUsingDDA, Waypoints)
186 {
187  pcl::PointCloud<pcl::PointXYZ> pc;
188  pc.push_back(pcl::PointXYZ(0.9, 0.6, 0.0));
189  // Add dummy points for getting map edges
190  pc.push_back(pcl::PointXYZ(-2.05, -2.05, -2.05));
191  pc.push_back(pcl::PointXYZ(2.05, 2.05, 2.05));
192 
194  kdtree->setInputCloud(pc.makeShared());
195  mcl_3dl::RaycastUsingDDA<pcl::PointXYZ> raycaster(0.1, 0.1, 0.1, 0.1, 0.5, 0.0);
196 
197  {
198  const std::vector<mcl_3dl::Vec3> expected_points =
199  {
200  mcl_3dl::Vec3(0.1, 0.0, 0.0), mcl_3dl::Vec3(0.1, 0.1, 0.0), mcl_3dl::Vec3(0.2, 0.1, 0.0),
201  mcl_3dl::Vec3(0.2, 0.2, 0.0), mcl_3dl::Vec3(0.3, 0.2, 0.0), mcl_3dl::Vec3(0.4, 0.2, 0.0),
202  mcl_3dl::Vec3(0.4, 0.3, 0.0), mcl_3dl::Vec3(0.5, 0.3, 0.0), mcl_3dl::Vec3(0.5, 0.4, 0.0),
203  mcl_3dl::Vec3(0.6, 0.4, 0.0), mcl_3dl::Vec3(0.7, 0.4, 0.0), mcl_3dl::Vec3(0.7, 0.5, 0.0),
204  mcl_3dl::Vec3(0.8, 0.5, 0.0), mcl_3dl::Vec3(0.8, 0.6, 0.0), mcl_3dl::Vec3(0.9, 0.6, 0.0),
205  };
206  compareRayWaypoints("Waypoints#1", kdtree, raycaster,
207  mcl_3dl::Vec3(0.0, 0.0, 0.0), mcl_3dl::Vec3(1.2, 0.8, 0.0),
208  expected_points, true);
209  }
210  {
211  // Start and end grids are same, but waypoints are different.
212  const std::vector<mcl_3dl::Vec3> expected_points =
213  {
214  mcl_3dl::Vec3(0.0, 0.1, 0.0), mcl_3dl::Vec3(0.1, 0.1, 0.0), mcl_3dl::Vec3(0.1, 0.2, 0.0),
215  mcl_3dl::Vec3(0.2, 0.2, 0.0), mcl_3dl::Vec3(0.3, 0.2, 0.0), mcl_3dl::Vec3(0.3, 0.3, 0.0),
216  mcl_3dl::Vec3(0.4, 0.3, 0.0), mcl_3dl::Vec3(0.4, 0.4, 0.0), mcl_3dl::Vec3(0.5, 0.4, 0.0),
217  mcl_3dl::Vec3(0.6, 0.4, 0.0), mcl_3dl::Vec3(0.6, 0.5, 0.0), mcl_3dl::Vec3(0.7, 0.5, 0.0),
218  mcl_3dl::Vec3(0.7, 0.6, 0.0), mcl_3dl::Vec3(0.8, 0.6, 0.0), mcl_3dl::Vec3(0.9, 0.6, 0.0),
219  };
220  compareRayWaypoints("Waypoints#2", kdtree, raycaster,
221  mcl_3dl::Vec3(-0.04, 0.04, 0.0), mcl_3dl::Vec3(1.16, 0.84, 0.0),
222  expected_points, true);
223  }
224 }
225 
226 TEST(RaycastUsingDDA, Intersection)
227 {
228  pcl::PointCloud<pcl::PointXYZ> pc;
229  pc.push_back(pcl::PointXYZ(0.6, -0.4, 0.0));
230  // Add dummy points for getting map edges
231  pc.push_back(pcl::PointXYZ(-2.1, -2.1, -2.1));
232  pc.push_back(pcl::PointXYZ(2.1, 2.1, 2.1));
233 
235  kdtree->setInputCloud(pc.makeShared());
236  mcl_3dl::RaycastUsingDDA<pcl::PointXYZ> raycaster(0.05, 0.05, 0.05, 0.2, 0.01, 0.0);
237  {
238  const std::vector<mcl_3dl::Vec3> expected_points =
239  {
240  mcl_3dl::Vec3(0.2, 0.0, 0.0), mcl_3dl::Vec3(0.2, -0.2, 0.0), mcl_3dl::Vec3(0.4, -0.2, 0.0),
241  mcl_3dl::Vec3(0.6, -0.2, 0.0), mcl_3dl::Vec3(0.6, -0.4, 0.0),
242  };
243  compareRayWaypoints("Intersection#1", kdtree, raycaster,
244  mcl_3dl::Vec3(0.0, 0.0, 0.0), mcl_3dl::Vec3(1.0, -0.55, 0.0),
245  expected_points, true);
246  }
247  {
248  const std::vector<mcl_3dl::Vec3> expected_points =
249  {
250  mcl_3dl::Vec3(0.2, 0.0, 0.0), mcl_3dl::Vec3(0.2, -0.2, 0.0), mcl_3dl::Vec3(0.4, -0.2, 0.0),
251  mcl_3dl::Vec3(0.6, -0.2, 0.0), mcl_3dl::Vec3(0.6, -0.4, 0.0), // The ray passes through this voxel
252  mcl_3dl::Vec3(0.8, -0.4, 0.0), mcl_3dl::Vec3(1.0, -0.4, 0.0),
253  };
254  // The ray does not hit the obstacle as the distance between them is larger than the threshold.
255  compareRayWaypoints("Intersection#2", kdtree, raycaster,
256  mcl_3dl::Vec3(0.0, 0.0, 0.0), mcl_3dl::Vec3(1.1, -0.55, 0.0),
257  expected_points, false);
258  }
259 }
260 
261 int main(int argc, char** argv)
262 {
263  testing::InitGoogleTest(&argc, argv);
264 
265  return RUN_ALL_TESTS();
266 }
f
TEST(RaycastUsingDDA, Collision)
std::shared_ptr< ChunkedKdtree > Ptr
void setInputCloud(const typename pcl::PointCloud< POINT_TYPE >::ConstPtr &cloud)
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void compareRayWaypoints(const char *name, typename mcl_3dl::ChunkedKdtree< pcl::PointXYZ >::Ptr kdtree, mcl_3dl::RaycastUsingDDA< pcl::PointXYZ > &raycaster, const mcl_3dl::Vec3 &ray_begin, const mcl_3dl::Vec3 &ray_end, const std::vector< mcl_3dl::Vec3 > expected_points, const bool expected_collision)
double epsilon
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void setRay(typename ChunkedKdtree< POINT_TYPE >::Ptr kdtree, const Vec3 ray_begin, const Vec3 ray_end_org) final
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool getNextCastResult(CastResult &result) final


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:16:29