test_mapcollection.cpp
Go to the documentation of this file.
1 
2 #include <stdio.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 OcTree* generateSphereTree(point3d origin, float radius){
12  OcTree* tree = new OcTree(0.05);
13 
14  point3d point_on_surface = origin;
15  point_on_surface.x() += radius;
16  for (int i=0; i<360; i++) {
17  for (int j=0; j<360; j++) {
18  if (!tree->insertRay(origin, origin+point_on_surface)) {
19  cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl;
20  }
21  point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
22  }
23  point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
24  }
25  return tree;
26 }
27 
28 int main(int argc, char** argv) {
29 
30  // //Generate a MapCollection
31  // MapCollection<MapNode<OcTree> > coll;
32  // OcTree* tree1 = generateSphereTree(point3d(0.01f,0.01f,0.01f), 2.0f);
33  // MapNode<OcTree>* mn1 = new MapNode<OcTree>(tree1, pose6d(0.0,0.0,0.0,0.0,0.0,0.0));
34  // mn1->setId("nodeone");
35  // coll.addNode(mn1);
36  // OcTree* tree2 = generateSphereTree(point3d(0.01f,0.01f,0.01f), 3.5f);
37  // MapNode<OcTree>* mn2 = new MapNode<OcTree>(tree2, pose6d(3.0,7.0,10.0,0.0,0.0,0.0));
38  // mn2->setId("nodetwo");
39  // coll.addNode(mn2);
40 
41  // //Write the MapCollection
42  // coll.write("tempcollection.txt");
43 
44  // //Read it back in
45  // MapCollection<MapNode<OcTree> > coll_read("tempcollection.txt");
46 
47  //TODO do some ray operations
48  //TODO do some isOccupied operations
49  //TODO do some comparisons between original and re-read MaCollection
50 
51  //Read MapCollection from command line
52  std::string filename(argv[1]);
53  MapCollection<MapNode<OcTree> > collection(filename);
54  EXPECT_TRUE(collection.size() > 0);
55 
56  //Write it to file
57  collection.write("writeout.txt");
58  //Write pointcloud to file
59  // collection.writePointcloud("test.vrml");
60 
61  //TODO delete temporary files?
62  //tempcollection.txt
63  //nodeone.bt
64  //nodetwo.bt
65  //writeout.txt
66  //test.vrml
67 
68  std::vector<point3d> query;
69  query.push_back(point3d(0,0,0));
70  query.push_back(point3d(2,0,0));
71  query.push_back(point3d(2,0,2));
72  query.push_back(point3d(1.99f,0.0f,0.0f));
73  query.push_back(point3d(0,0,3));
74  query.push_back(point3d(3,7,13.5));
75  query.push_back(point3d(0,-1,-1));
76 
77  for (std::vector<point3d>::iterator it = query.begin(); it != query.end(); ++it) {
78  point3d& q = *it;
79  if (collection.isOccupied(q))
80  printf("q (%0.2f, %0.2f, %0.2f) is occupied\n", q.x(), q.y(), q.z());
81  else
82  printf("q (%0.2f, %0.2f, %0.2f) is NOT occupied\n", q.x(), q.y(), q.z());
83  printf("in fact, it has an occupancy probability of %0.2f\n", collection.getOccupancy(q));
84  }
85 
86  point3d ray_origin (0,0,10);
87  point3d ray_direction (0,0,-10);
88  point3d ray_end (100,100,100);
89 
90  if (collection.castRay(ray_origin, ray_direction, ray_end,true)) {
91  printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f hit obstacle at %.2f,%.2f,%.2f\n",
92  ray_origin.x(), ray_origin.y(), ray_origin.z(),
93  ray_direction.x(), ray_direction.y(), ray_direction.z(),
94  ray_end.x(), ray_end.y(), ray_end.z());
95  }
96  else {
97  printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f FAIL\n",
98  ray_origin.x(), ray_origin.y(), ray_origin.z(),
99  ray_direction.x(), ray_direction.y(), ray_direction.z());
100  }
101 
102 
103  printf("\n\n");
104 
105  ray_origin = point3d(0,0,-10);
106  ray_direction = point3d(0,0,10);
107 
108  if (collection.castRay(ray_origin, ray_direction, ray_end,true)) {
109  printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f hit obstacle at %.2f,%.2f,%.2f\n",
110  ray_origin.x(), ray_origin.y(), ray_origin.z(),
111  ray_direction.x(), ray_direction.y(), ray_direction.z(),
112  ray_end.x(), ray_end.y(), ray_end.z());
113  }
114  else {
115  printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f FAIL\n",
116  ray_origin.x(), ray_origin.y(), ray_origin.z(),
117  ray_direction.x(), ray_direction.y(), ray_direction.z());
118  }
119 
120  printf("\n\n");
121 
122  //....
123 
124  ray_origin = point3d(0,-1.5,-3);
125  ray_direction = point3d(0,0,1);
126 
127  if (collection.castRay(ray_origin, ray_direction, ray_end,true, 5)) {
128  printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f hit obstacle at %.2f,%.2f,%.2f\n",
129  ray_origin.x(), ray_origin.y(), ray_origin.z(),
130  ray_direction.x(), ray_direction.y(), ray_direction.z(),
131  ray_end.x(), ray_end.y(), ray_end.z());
132  }
133  else {
134  printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f FAIL\n",
135  ray_origin.x(), ray_origin.y(), ray_origin.z(),
136  ray_direction.x(), ray_direction.y(), ray_direction.z());
137  }
138 
139 
140  return 0;
141 }
Vector3 & rotate_IP(double roll, double pitch, double yaw)
Definition: Vector3.cpp:41
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
float & y()
Definition: Vector3.h:136
double getOccupancy(const point3d &p)
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:49
#define DEG2RAD(x)
Definition: Utils.h:47
int main(int argc, char **argv)
OcTree * generateSphereTree(point3d origin, float radius)
bool write(std::string filename)
bool isOccupied(const point3d &p) const
#define EXPECT_TRUE(args)
Definition: testing.h:6
float & z()
Definition: Vector3.h:141
size_t size() const
Definition: MapCollection.h:79


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06