test_raycasting.cpp
Go to the documentation of this file.
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 
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 
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 }
int main(int argc, char **argv)
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
NODE * search(double x, double y, double z, unsigned int depth=0) const
#define M_PI
Definition: Utils.h:38
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
#define EXPECT_NEAR(a, b, prec)
Definition: testing.h:27
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
Vector3 & rotate_IP(double roll, double pitch, double yaw)
Definition: Vector3.cpp:41
#define EXPECT_FALSE(args)
Definition: testing.h:11
#define EXPECT_FLOAT_EQ(a, b)
Definition: testing.h:22
#define EXPECT_EQ(a, b)
Definition: testing.h:16
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
virtual bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
bool writeBinary(const std::string &filename)
float & y()
Definition: Vector3.h:136
float & x()
Definition: Vector3.h:131
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:48
#define DEG2RAD(x)
Definition: Utils.h:47
#define EXPECT_TRUE(args)
Definition: testing.h:6
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
float & z()
Definition: Vector3.h:141


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Fri Mar 25 2016 03:52:55