unit_tests.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <string>
3 #ifdef _WIN32
4  #include <Windows.h> // to define Sleep()
5 #else
6  #include <unistd.h> // POSIX sleep()
7 #endif
8 
9 
10 #include <octomap/octomap.h>
11 #include <octomap/OcTreeStamped.h>
12 #include <octomap/math/Utils.h>
13 #include "testing.h"
14 
15 using namespace std;
16 using namespace octomap;
17 using namespace octomath;
18 
19 int main(int argc, char** argv) {
20 
21  if (argc != 2){
22  std::cerr << "Error: you need to specify a test as argument" << std::endl;
23  return 1; // exit 1 means failure
24  }
25  std::string test_name (argv[1]);
26 
27 
28  // ------------------------------------------------------------
29  if (test_name == "MathVector") {
30  // test constructors
31  Vector3* twos = new Vector3();
32  Vector3* ones = new Vector3(1,1,1);
33  for (int i=0;i<3;i++) {
34  (*twos)(i) = 2;
35  }
36  // test basic operations
37  Vector3 subtraction = *twos - *ones;
38  Vector3 addition = *twos + *ones;
39  Vector3 multiplication = *twos * 2.;
40 
41  for (int i=0;i<3;i++) {
42  EXPECT_FLOAT_EQ (subtraction(i), 1.);
43  EXPECT_FLOAT_EQ (addition(i), 3.);
44  EXPECT_FLOAT_EQ (multiplication(i), 4.);
45  }
46 
47  // copy constructor
48  Vector3 rotation = *ones;
49 
50  // rotation
51  rotation.rotate_IP (M_PI, 1., 0.1);
52  EXPECT_FLOAT_EQ (rotation.x(), 1.2750367);
53  EXPECT_FLOAT_EQ (rotation.y(), (-1.1329513));
54  EXPECT_FLOAT_EQ (rotation.z(), 0.30116868);
55 
56  // ------------------------------------------------------------
57  } else if (test_name == "MathPose") {
58  // constructors
59  Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (float) M_PI/4. );
60  Pose6D b;
61 
62  Vector3 trans(1.0f, 0.1f, 0.1f);
63  Quaternion rot(0.0f, 0.1f, (float) M_PI/4.);
64  Pose6D c(trans, rot);
65 
66  // comparator
67  EXPECT_TRUE ( a == c);
68  // toEuler
69  EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.);
70 
71  // transform
72  Vector3 t = c.transform (trans);
73  EXPECT_FLOAT_EQ (t.x() , 1.6399229);
74  EXPECT_FLOAT_EQ (t.y() , 0.8813442);
75  EXPECT_FLOAT_EQ (t.z() , 0.099667005);
76 
77  // inverse transform
78  Pose6D c_inv = c.inv();
79  Vector3 t2 = c_inv.transform (t);
80  EXPECT_FLOAT_EQ (t2.x() , trans.x());
81  EXPECT_FLOAT_EQ (t2.y() , trans.y());
82  EXPECT_FLOAT_EQ (t2.z() , trans.z());
83 
84  // ------------------------------------------------------------
85  } else if (test_name == "InsertRay") {
86  double p = 0.5;
88  p = 0.1;
90  p = 0.99;
92 
93  float l = 0;
95  l = -4;
97  l = 2;
99 
100 
101  OcTree tree (0.05);
102  tree.setProbHit(0.7);
103  tree.setProbMiss(0.4);
104 
105  point3d origin (0.01f, 0.01f, 0.02f);
106  point3d point_on_surface (2.01f,0.01f,0.01f);
107 
108  for (int i=0; i<360; i++) {
109  for (int j=0; j<360; j++) {
110  EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface));
111  point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
112  }
113  point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
114  }
115  EXPECT_TRUE (tree.writeBinary("sphere_rays.bt"));
116  EXPECT_EQ ((int) tree.size(), 50615);
117 
118  // ------------------------------------------------------------
119  // ray casting is now in "test_raycasting.cpp"
120 
121  // ------------------------------------------------------------
122  // insert scan test
123  // insert graph node test
124  // write graph test
125  } else if (test_name == "InsertScan") {
126  Pointcloud* measurement = new Pointcloud();
127 
128  point3d origin (0.01f, 0.01f, 0.02f);
129  point3d point_on_surface (2.01f, 0.01f, 0.01f);
130 
131  for (int i=0; i<360; i++) {
132  for (int j=0; j<360; j++) {
133  point3d p = origin+point_on_surface;
134  measurement->push_back(p);
135  point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
136  }
137  point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
138  }
139 
140  OcTree tree (0.05);
141  tree.insertPointCloud(*measurement, origin);
142  EXPECT_EQ (tree.size(), 53959);
143 
144  ScanGraph* graph = new ScanGraph();
145  Pose6D node_pose (origin.x(), origin.y(), origin.z(),0.0f,0.0f,0.0f);
146  graph->addNode(measurement, node_pose);
147  EXPECT_TRUE (graph->writeBinary("test.graph"));
148  delete graph;
149  // ------------------------------------------------------------
150  // graph read file test
151  } else if (test_name == "ReadGraph") {
152  ScanGraph graph;
153  EXPECT_TRUE (graph.readBinary("test.graph"));
154  // ------------------------------------------------------------
155 
156  } else if (test_name == "StampedTree") {
157  OcTreeStamped stamped_tree (0.05);
158  // fill tree
159  for (int x=-20; x<20; x++)
160  for (int y=-20; y<20; y++)
161  for (int z=-20; z<20; z++) {
162  point3d p ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
163  stamped_tree.updateNode(p, true); // integrate 'occupied' measurement
164  }
165  // test if update times set
166  point3d query (0.1f, 0.1f, 0.1f);
167  OcTreeNodeStamped* result = stamped_tree.search (query);
168  EXPECT_TRUE (result);
169  unsigned int tree_time = stamped_tree.getLastUpdateTime();
170  unsigned int node_time = result->getTimestamp();
171  std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl;
172  EXPECT_TRUE (tree_time > 0);
173  #ifdef _WIN32
174  Sleep(1000);
175  #else
176  sleep(1);
177  #endif
178  stamped_tree.integrateMissNoTime(result); // reduce occupancy, no time update
179  std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl;
180  EXPECT_EQ (node_time, result->getTimestamp()); // node time updated?
181  point3d query2 = point3d (0.1f, 0.1f, 0.3f);
182  stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement
183  OcTreeNodeStamped* result2 = stamped_tree.search (query2);
184  EXPECT_TRUE (result2);
185  result = stamped_tree.search (query);
186  EXPECT_TRUE (result);
187  std::cout << "After 3rd update (single hit at (0.1, 0.1, 0.3): Tree time " << stamped_tree.getLastUpdateTime() << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp()
188  << "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl;
189  EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated
190  EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime());
191  // ------------------------------------------------------------
192  } else if (test_name == "OcTreeKey") {
193  OcTree tree (0.05);
194  point3d p(0.0,0.0,0.0);
195  OcTreeKey key;
196  tree.coordToKeyChecked(p, key);
197  point3d p_inv = tree.keyToCoord(key);
198  EXPECT_FLOAT_EQ (0.025, p_inv.x());
199  EXPECT_FLOAT_EQ (0.025, p_inv.y());
200  EXPECT_FLOAT_EQ (0.025, p_inv.z());
201 
202  // ------------------------------------------------------------
203  } else {
204  std::cerr << "Invalid test name specified: " << test_name << std::endl;
205  return 1;
206 
207  }
208 
209  std::cerr << "Test successful.\n";
210  return 0;
211 }
std::istream & readBinary(std::ifstream &s)
Definition: ScanGraph.cpp:365
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
Definition: Pose6D.cpp:80
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
void integrateMissNoTime(OcTreeNodeStamped *node) const
void setProbHit(double prob)
sets the probability for a "hit" (will be converted to logodds) - sensor model
double probability(double logodds)
compute probability from logodds:
Definition: octomap_utils.h:47
NODE * search(double x, double y, double z, unsigned int depth=0) const
int main(int argc, char **argv)
Definition: unit_tests.cpp:19
#define M_PI
Definition: Utils.h:38
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
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
unsigned int getLastUpdateTime()
Pose6D inv() const
Inversion.
Definition: Pose6D.cpp:65
unsigned int getTimestamp() const
Definition: OcTreeStamped.h:62
void setProbMiss(double prob)
sets the probability for a "miss" (will be converted to logodds) - sensor model
#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)
bool writeBinary(const std::string &filename)
float & y()
Definition: Vector3.h:136
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
float & x()
Definition: Vector3.h:131
double yaw() const
Definition: Pose6D.h:109
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
float logodds(double probability)
compute log-odds from probability:
Definition: octomap_utils.h:42
#define EXPECT_TRUE(args)
Definition: testing.h:6
float & z()
Definition: Vector3.h:141
virtual size_t size() const
This class represents a Quaternion.
Definition: Quaternion.h:56
double keyToCoord(key_type key, unsigned depth) const


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Jun 5 2019 19:26:27