11 std::cerr <<
"\nUSAGE: " <<
self <<
" spherical_scan.graph (reference file to compare, required)\n\n";
16 int main(
int argc,
char** argv) {
21 std::string filename = std::string(argv[1]);
31 point3d point_on_surface (4.01f, 0.01f, 0.01f);
36 for (
int i=-50; i<51; i++) {
37 for (
int j=-50; j<51; j++) {
38 point3d rotated = point_on_surface;
44 pose6d origin(1.0, 0, -0.5, 0, 0, 0);
50 std::cout <<
"Comparing ScanGraph with reference file at " << filename << std::endl;
59 for (
size_t i = 0; i < scanNode->scan->size(); ++i){
66 std::cout <<
"Testing ScanGraph I/O" << std::endl;
71 EXPECT_TRUE(reReadGraph.readBinary(
"spherical_scan_out.graph"));
77 ScanNode* readScanNode = *reReadGraph.begin();
83 for (
size_t i = 0; i < scanNode->scan->size(); ++i){
99 cout <<
"Test done." << endl;
ScanNode * addNode(Pointcloud *scan, pose6d pose)
std::istream & readBinary(std::ifstream &s)
pose6d pose
6D pose from which the scan was performed
std::ostream & writeBinary(std::ostream &s) const
void push_back(float x, float y, float z)
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)
bool writeBinary(const std::string &filename)
void printUsage(char *self)
This class represents a tree-dimensional pose of an object.
Vector3 & trans()
Translational component.
int main(int argc, char **argv)
This class represents a three-dimensional vector.
#define EXPECT_TRUE(args)