test_chunked_kdtree.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, 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 <cstddef>
31 #include <cmath>
32 #include <vector>
33 
34 #include <gtest/gtest.h>
35 
36 #include <mcl_3dl/chunked_kdtree.h>
37 
38 TEST(ChunkedKdtree, RadiusSearch)
39 {
40  pcl::PointCloud<pcl::PointXYZ> pc;
41  pc.push_back(pcl::PointXYZ(0.5, 0.5, 0.5)); // 0
42  pc.push_back(pcl::PointXYZ(0.8, 0.0, 0.0)); // 1
43  pc.push_back(pcl::PointXYZ(1.3, 0.0, 0.0)); // 2
44  pc.push_back(pcl::PointXYZ(0.0, 0.2, 0.0)); // 3
45  pc.push_back(pcl::PointXYZ(0.0, -0.3, 0.0)); // 4
46 
48  kdtree.setInputCloud(pc.makeShared());
49 
50  std::vector<int> id;
51  std::vector<float> dist;
52 
53  kdtree.radiusSearch(
54  pcl::PointXYZ(0.5, 0.5, 0.5),
55  0.3, id, dist, 1);
56  ASSERT_EQ(id.size(), 1u);
57  ASSERT_EQ(id[0], 0);
58 
59  kdtree.radiusSearch(
60  pcl::PointXYZ(0.5, 0.4, 0.5),
61  0.3, id, dist, 1);
62  ASSERT_EQ(id.size(), 1u);
63  ASSERT_EQ(id[0], 0);
64 
65  kdtree.radiusSearch(
66  pcl::PointXYZ(1.05, 0.0, 0.0),
67  0.3, id, dist, 1);
68  ASSERT_EQ(id.size(), 1u);
69  ASSERT_EQ(id[0], 1);
70 
71  kdtree.radiusSearch(
72  pcl::PointXYZ(1.1, 0.0, 0.0),
73  0.3, id, dist, 1);
74  ASSERT_EQ(id.size(), 1u);
75  ASSERT_EQ(id[0], 2);
76 
77  kdtree.radiusSearch(
78  pcl::PointXYZ(0.0, -0.05, 0.0),
79  0.3, id, dist, 1);
80  ASSERT_EQ(id.size(), 1u);
81  ASSERT_EQ(id[0], 3);
82 
83  kdtree.radiusSearch(
84  pcl::PointXYZ(0.0, -0.15, 0.0),
85  0.3, id, dist, 1);
86  ASSERT_EQ(id.size(), 1u);
87  ASSERT_EQ(id[0], 4);
88 }
89 
90 int main(int argc, char** argv)
91 {
92  testing::InitGoogleTest(&argc, argv);
93 
94  return RUN_ALL_TESTS();
95 }
void setInputCloud(const typename pcl::PointCloud< POINT_TYPE >::ConstPtr &cloud)
TEST(ChunkedKdtree, RadiusSearch)
int radiusSearch(const POINT_TYPE &p, const float &radius, std::vector< int > &id, std::vector< float > &dist_sq, const size_t &num)
int main(int argc, char **argv)


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