1
2 #include <stdio.h>
3 #include <octomap/octomap.h>
4 #include <octomap/math/Utils.h>
5 #include "testing.h"
6
7 using namespace std;
8 using namespace octomap;
9 using namespace octomath;
10
11
12
main(int,char **)13 int main(int /*argc*/, char** /*argv*/) {
14
15
16
17 OcTree tree (0.05);
18
19
20
21 // point3d origin (10.01, 10.01, 10.02);
22 point3d origin (0.01f, 0.01f, 0.02f);
23 point3d point_on_surface (2.01f, 0.01f, 0.01f);
24
25 cout << "Generating sphere at " << origin << " ..." << endl;
26
27 unsigned sphere_beams = 500;
28 double angle = 2.0*M_PI/double(sphere_beams);
29 Pointcloud p;
30 for (unsigned i=0; i<sphere_beams; i++) {
31 for (unsigned j=0; j<sphere_beams; j++) {
32 p.push_back(origin+point_on_surface);
33 point_on_surface.rotate_IP (0,0,angle);
34 }
35 point_on_surface.rotate_IP (0,angle,0);
36 }
37 tree.insertPointCloud(p, origin);
38
39
40 cout << "Writing to sphere.bt..." << endl;
41 EXPECT_TRUE(tree.writeBinary("sphere.bt"));
42
43 // -----------------------------------------------
44
45 cout << "Casting rays in sphere ..." << endl;
46
47 OcTree sampled_surface (0.05);
48
49 point3d direction = point3d (1.0,0.0,0.0);
50 point3d obstacle(0,0,0);
51
52 unsigned int hit (0);
53 unsigned int miss (0);
54 unsigned int unknown (0);
55 double mean_dist(0);
56
57 for (unsigned i=0; i<sphere_beams; i++) {
58 for (unsigned j=0; j<sphere_beams; j++) {
59 if (!tree.castRay(origin, direction, obstacle, false, 3.)) {
60 // hit unknown
61 if (!tree.search(obstacle))
62 unknown++;
63 else
64 miss++;
65 }
66 else {
67 hit++;
68 mean_dist += (obstacle - origin).norm();
69 sampled_surface.updateNode(obstacle, true);
70 }
71 direction.rotate_IP (0,0,angle);
72 }
73 direction.rotate_IP (0,angle,0);
74 }
75
76 cout << "Writing sampled_surface.bt" << endl;
77 EXPECT_TRUE(sampled_surface.writeBinary("sampled_surface.bt"));
78
79 mean_dist /= (double) hit;
80 std::cout << " hits / misses / unknown: " << hit << " / " << miss << " / " << unknown << std::endl;
81 std::cout << " mean obstacle dist: " << mean_dist << std::endl;
82 EXPECT_NEAR(mean_dist, 2., 0.05);
83 EXPECT_EQ(hit, (sphere_beams*sphere_beams));
84 EXPECT_EQ(miss, 0);
85 EXPECT_EQ(unknown, 0);
86
87
88 // -----------------------------------------------
89
90 cout << "generating single rays..." << endl;
91 OcTree single_beams(0.03333);
92 int num_beams = 17;
93 float beamLength = 10.0f;
94 point3d single_origin (1.0f, 0.45f, 0.45f);
95 point3d single_origin_top (1.0f, 0.45f, 1.0);
96 point3d single_endpoint(beamLength, 0.0f, 0.0f);
97
98
99 for (int i=0; i<num_beams; i++) {
100 for (int j=0; j<num_beams; j++) {
101 if (!single_beams.insertRay(single_origin, single_origin+single_endpoint)) {
102 cout << "ERROR while inserting ray from " << single_origin << " to " << single_endpoint << endl;
103 }
104 single_endpoint.rotate_IP (0,0,DEG2RAD(360.0/num_beams));
105 }
106 single_endpoint.rotate_IP (0,DEG2RAD(360.0/num_beams),0);
107 }
108
109
110 cout << "done." << endl;
111 cout << "writing to beams.bt..." << endl;
112 EXPECT_TRUE(single_beams.writeBinary("beams.bt"));
113
114
115 ////// more tests from unit_tests.cpp:
116 double res = 0.1;
117 double res_2 = res/2.0;
118 OcTree cubeTree(res);
119 // fill a cube with "free", end is "occupied":
120 for (float x=-0.95f; x <= 1.0f; x+=float(res)){
121 for (float y=-0.95f; y <= 1.0f; y+= float(res)){
122 for (float z=-0.95f; z <= 1.0f; z+= float(res)){
123 if (x < 0.9){
124 EXPECT_TRUE(cubeTree.updateNode(point3d(x,y,z), false));
125 } else{
126 EXPECT_TRUE(cubeTree.updateNode(point3d(x,y,z), true));
127 }
128 }
129 }
130 }
131
132 // fill some "floor":
133 EXPECT_TRUE(cubeTree.updateNode(res_2,res_2,-res_2, true));
134 EXPECT_TRUE(cubeTree.updateNode(3*res_2,res_2,-res_2, true));
135 EXPECT_TRUE(cubeTree.updateNode(-res_2,res_2,-res_2, true));
136 EXPECT_TRUE(cubeTree.updateNode(-3*res_2,res_2,-res_2, true));
137
138 cubeTree.writeBinary("raycasting_cube.bt");
139 origin = point3d(0.0f, 0.0f, 0.0f);
140 point3d end;
141 // hit the corner:
142 direction = point3d(0.95f, 0.95f, 0.95f);
143 EXPECT_TRUE(cubeTree.castRay(origin, direction, end, false));
144 EXPECT_TRUE(cubeTree.isNodeOccupied(cubeTree.search(end)));
145 std::cout << "Hit occupied voxel: " << end << std::endl;
146 direction = point3d(1.0, 0.0, 0.0);
147 EXPECT_TRUE(cubeTree.castRay(origin, direction, end, false));
148 EXPECT_TRUE(cubeTree.isNodeOccupied(cubeTree.search(end)));
149 std::cout << "Hit occupied voxel: " << end << std::endl;
150 EXPECT_NEAR(1.0, (origin - end).norm(), res_2);
151
152 // hit bottom:
153 origin = point3d(float(res_2), float(res_2), 0.5f);
154 direction = point3d(0.0, 0.0, -1.0f);
155 EXPECT_TRUE(cubeTree.castRay(origin, direction, end, false));
156 EXPECT_TRUE(cubeTree.isNodeOccupied(cubeTree.search(end)));
157 std::cout << "Hit voxel: " << end << std::endl;
158 EXPECT_FLOAT_EQ(origin(0), end(0));
159 EXPECT_FLOAT_EQ(origin(1), end(1));
160 EXPECT_FLOAT_EQ(-res_2, end(2));
161
162
163 // hit boundary of unknown:
164 origin = point3d(0.0f, 0.0f, 0.0f);
165 direction = point3d(0.0f, 1.0f, 0.0f);
166 EXPECT_FALSE(cubeTree.castRay(origin, direction, end, false));
167 EXPECT_FALSE(cubeTree.search(end));
168 std::cout << "Boundary unknown hit: " << end << std::endl;
169
170 // hit boundary of octree:
171 EXPECT_FALSE(cubeTree.castRay(origin, direction, end, true));
172 EXPECT_FALSE(cubeTree.search(end));
173 EXPECT_FLOAT_EQ(end.x(), res_2);
174 EXPECT_FLOAT_EQ(end.y(), float(32768*res-res_2));
175 EXPECT_FLOAT_EQ(end.z(), res_2);
176 direction = point3d(-1.0f, 0.0f, 0.0f);
177 EXPECT_FALSE(cubeTree.castRay(origin, direction, end, true));
178 EXPECT_FALSE(cubeTree.search(end));
179 EXPECT_FLOAT_EQ(end.x(), float(-32767*res-res_2));
180 EXPECT_FLOAT_EQ(end.y(), res_2);
181 EXPECT_FLOAT_EQ(end.z(), res_2);
182
183 // test maxrange:
184 EXPECT_FALSE(cubeTree.castRay(origin, direction, end, true, 0.9));
185 std::cout << "Max range endpoint: " << end << std::endl;
186 OcTreeNode* endPt = cubeTree.search(end);
187 EXPECT_TRUE(endPt);
188 EXPECT_FALSE(cubeTree.isNodeOccupied(endPt));
189 double dist = (origin - end).norm();
190 EXPECT_NEAR(0.9, dist, res);
191
192
193 std::cout << "Test successful\n";
194 return 0;
195 }
196