test_scans.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 
10 void printUsage(char* self){
11  std::cerr << "\nUSAGE: " << self << " spherical_scan.graph (reference file to compare, required)\n\n";
12 
13  exit(1);
14 }
15 
16 int main(int argc, char** argv) {
17  if (argc != 2){
18  printUsage(argv[0]);
19  }
20 
21  std::string filename = std::string(argv[1]);
22 
23  ScanGraph referenceGraph;
24  EXPECT_TRUE(referenceGraph.readBinary(filename));
25 
26  // TODO: read in reference graph file
27 
28 
29  //##############################################################
30 
31  point3d point_on_surface (4.01f, 0.01f, 0.01f);
32 
33 
34  Pointcloud* cloud = new Pointcloud();
35 
36  for (int i=-50; i<51; i++) {
37  for (int j=-50; j<51; j++) {
38  point3d rotated = point_on_surface;
39  rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
40  cloud->push_back(rotated);
41  }
42  }
43 
44  pose6d origin(1.0, 0, -0.5, 0, 0, 0);
45 
46  ScanGraph graph;
47  graph.addNode(cloud, origin); // graph assumes ownership of cloud!
48 
49  {
50  std::cout << "Comparing ScanGraph with reference file at " << filename << std::endl;
51  EXPECT_TRUE(graph.size() == referenceGraph.size());
52  ScanNode* scanNode = *graph.begin();
53  ScanNode* refScanNode = *referenceGraph.begin();
54 
55  EXPECT_EQ(scanNode->id, refScanNode->id);
56  EXPECT_EQ(scanNode->pose, refScanNode->pose);
57  EXPECT_EQ(scanNode->scan->size(), refScanNode->scan->size());
58 
59  for (size_t i = 0; i < scanNode->scan->size(); ++i){
60  EXPECT_EQ((*scanNode->scan)[i], (*refScanNode->scan)[i]);
61  }
62 
63  }
64  // test reading and writing to file
65  {
66  std::cout << "Testing ScanGraph I/O" << std::endl;
67 
68  EXPECT_TRUE(graph.writeBinary("spherical_scan_out.graph"));
69 
70  ScanGraph reReadGraph;
71  EXPECT_TRUE(reReadGraph.readBinary("spherical_scan_out.graph"));
72 
73  EXPECT_TRUE(graph.size() == reReadGraph.size());
74  EXPECT_EQ(reReadGraph.size(), 1);
75 
76  ScanNode* scanNode = *graph.begin();
77  ScanNode* readScanNode = *reReadGraph.begin();
78 
79  EXPECT_EQ(scanNode->id, readScanNode->id);
80  EXPECT_EQ(scanNode->pose, readScanNode->pose);
81  EXPECT_EQ(scanNode->scan->size(), readScanNode->scan->size());
82 
83  for (size_t i = 0; i < scanNode->scan->size(); ++i){
84  EXPECT_EQ((*scanNode->scan)[i], (*readScanNode->scan)[i]);
85  }
86  }
87 
88 
89  // insert into OcTree
90  {
91  OcTree tree (0.05);
92 
93  // insert in global coordinates:
94  tree.insertPointCloud(*cloud, origin.trans());
95 
96  tree.writeBinary("spherical_scan.bt");
97  }
98 
99  cout << "Test done." << endl;
100  exit(0);
101 
102 }
iterator begin()
Definition: ScanGraph.h:175
ScanNode * addNode(Pointcloud *scan, pose6d pose)
Definition: ScanGraph.cpp:179
size_t size() const
Definition: ScanGraph.h:180
std::istream & readBinary(std::ifstream &s)
Definition: ScanGraph.cpp:369
Pointcloud * scan
Definition: ScanGraph.h:73
pose6d pose
6D pose from which the scan was performed
Definition: ScanGraph.h:74
std::ostream & writeBinary(std::ostream &s) const
Definition: ScanGraph.cpp:328
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
unsigned int id
Definition: ScanGraph.h:75
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
size_t size() const
Definition: Pointcloud.h:57
#define EXPECT_EQ(a, b)
Definition: testing.h:16
bool writeBinary(const std::string &filename)
void printUsage(char *self)
Definition: test_scans.cpp:10
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
Vector3 & trans()
Translational component.
Definition: Pose6D.h:80
int main(int argc, char **argv)
Definition: test_scans.cpp:16
This class represents a three-dimensional vector.
Definition: Vector3.h:50
#define DEG2RAD(x)
Definition: Utils.h:47
#define EXPECT_TRUE(args)
Definition: testing.h:6


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