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


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Tue Dec 12 2023 03:39:41