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  // not really meaningful, see better test in "test_scans.cpp"
153  ScanGraph graph;
154  EXPECT_TRUE (graph.readBinary("test.graph"));
155  // ------------------------------------------------------------
156 
157  } else if (test_name == "StampedTree") {
158  OcTreeStamped stamped_tree (0.05);
159  // fill tree
160  for (int x=-20; x<20; x++)
161  for (int y=-20; y<20; y++)
162  for (int z=-20; z<20; z++) {
163  point3d p ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
164  stamped_tree.updateNode(p, true); // integrate 'occupied' measurement
165  }
166  // test if update times set
167  point3d query (0.1f, 0.1f, 0.1f);
168  OcTreeNodeStamped* result = stamped_tree.search (query);
169  EXPECT_TRUE (result);
170  unsigned int tree_time = stamped_tree.getLastUpdateTime();
171  unsigned int node_time = result->getTimestamp();
172  std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl;
173  EXPECT_TRUE (tree_time > 0);
174  #ifdef _WIN32
175  Sleep(1000);
176  #else
177  sleep(1);
178  #endif
179  stamped_tree.integrateMissNoTime(result); // reduce occupancy, no time update
180  std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl;
181  EXPECT_EQ (node_time, result->getTimestamp()); // node time updated?
182  point3d query2 = point3d (0.1f, 0.1f, 0.3f);
183  stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement
184  OcTreeNodeStamped* result2 = stamped_tree.search (query2);
185  EXPECT_TRUE (result2);
186  result = stamped_tree.search (query);
187  EXPECT_TRUE (result);
188  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()
189  << "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl;
190  EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated
191  EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime());
192  // ------------------------------------------------------------
193  } else if (test_name == "OcTreeKey") {
194  OcTree tree (0.05);
195  point3d p(0.0,0.0,0.0);
196  OcTreeKey key;
197  tree.coordToKeyChecked(p, key);
198  point3d p_inv = tree.keyToCoord(key);
199  EXPECT_FLOAT_EQ (0.025, p_inv.x());
200  EXPECT_FLOAT_EQ (0.025, p_inv.y());
201  EXPECT_FLOAT_EQ (0.025, p_inv.z());
202 
203  // ------------------------------------------------------------
204  } else {
205  std::cerr << "Invalid test name specified: " << test_name << std::endl;
206  return 1;
207 
208  }
209 
210  std::cerr << "Test successful.\n";
211  return 0;
212 }
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
Definition: Pose6D.cpp:80
std::istream & readBinary(std::ifstream &s)
Definition: ScanGraph.cpp:369
void integrateMissNoTime(OcTreeNodeStamped *node) const
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
void setProbHit(double prob)
sets the probability for a "hit" (will be converted to logodds) - sensor model
double keyToCoord(key_type key, unsigned depth) const
double probability(double logodds)
compute probability from logodds:
Definition: octomap_utils.h:47
int main(int argc, char **argv)
Definition: unit_tests.cpp:19
#define M_PI
Definition: Utils.h:38
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()
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
NODE * search(double x, double y, double z, unsigned int depth=0) const
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
double yaw() const
Definition: Pose6D.h:109
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
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
virtual size_t size() const
#define DEG2RAD(x)
Definition: Utils.h:47
float logodds(double probability)
compute log-odds from probability:
Definition: octomap_utils.h:42
unsigned int getTimestamp() const
Definition: OcTreeStamped.h:62
#define EXPECT_TRUE(args)
Definition: testing.h:6
float & z()
Definition: Vector3.h:141
Pose6D inv() const
Inversion.
Definition: Pose6D.cpp:65
This class represents a Quaternion.
Definition: Quaternion.h:56


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Fri Jun 7 2019 21:17:32