unit_tests.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <string>
00003 #ifdef _WIN32
00004   #include <Windows.h>  // to define Sleep()
00005 #else
00006   #include <unistd.h>   // POSIX sleep()
00007 #endif
00008 
00009 
00010 #include <octomap/octomap.h>
00011 #include <octomap/OcTreeStamped.h>
00012 #include <octomap/math/Utils.h>
00013 #include "testing.h"
00014  
00015 using namespace std;
00016 using namespace octomap;
00017 using namespace octomath;
00018 
00019 int main(int argc, char** argv) {
00020 
00021   if (argc != 2){
00022     std::cerr << "Error: you need to specify a test as argument" << std::endl;
00023     return 1; // exit 1 means failure
00024   }
00025   std::string test_name (argv[1]);
00026 
00027 
00028   // ------------------------------------------------------------
00029   if (test_name == "MathVector") {
00030     // test constructors
00031     Vector3* twos = new Vector3();        
00032     Vector3* ones = new Vector3(1,1,1);    
00033     for (int i=0;i<3;i++) {
00034       (*twos)(i) = 2;
00035     }  
00036     // test basic operations
00037     Vector3 subtraction = *twos - *ones;
00038     Vector3 addition = *twos + *ones;
00039     Vector3 multiplication = *twos * 2.;
00040   
00041     for (int i=0;i<3;i++) {
00042       EXPECT_FLOAT_EQ (subtraction(i), 1.);
00043       EXPECT_FLOAT_EQ (addition(i), 3.);
00044       EXPECT_FLOAT_EQ (multiplication(i), 4.);
00045     }
00046 
00047     // copy constructor
00048     Vector3 rotation =  *ones;
00049 
00050     // rotation
00051     rotation.rotate_IP (M_PI, 1., 0.1);
00052     EXPECT_FLOAT_EQ (rotation.x(), 1.2750367);
00053     EXPECT_FLOAT_EQ (rotation.y(), (-1.1329513));
00054     EXPECT_FLOAT_EQ (rotation.z(), 0.30116868);
00055   
00056   // ------------------------------------------------------------
00057   } else if (test_name == "MathPose") {
00058     // constructors  
00059     Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (float) M_PI/4. );
00060     Pose6D b;
00061 
00062     Vector3 trans(1.0f, 0.1f, 0.1f);
00063     Quaternion rot(0.0f, 0.1f, (float) M_PI/4.);
00064     Pose6D c(trans, rot);
00065 
00066     // comparator
00067     EXPECT_TRUE ( a == c);
00068     // toEuler
00069     EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.);
00070 
00071     // transform
00072     Vector3 t = c.transform (trans);
00073     EXPECT_FLOAT_EQ (t.x() , 1.6399229);
00074     EXPECT_FLOAT_EQ (t.y() , 0.8813442);
00075     EXPECT_FLOAT_EQ (t.z() , 0.099667005);
00076 
00077     // inverse transform
00078     Pose6D c_inv = c.inv();
00079     Vector3 t2 = c_inv.transform (t);
00080     EXPECT_FLOAT_EQ (t2.x() , trans.x());
00081     EXPECT_FLOAT_EQ (t2.y() , trans.y());
00082     EXPECT_FLOAT_EQ (t2.z() , trans.z());
00083 
00084   // ------------------------------------------------------------
00085   } else if (test_name == "InsertRay") {
00086     double p = 0.5;
00087     EXPECT_FLOAT_EQ(p, probability(logodds(p)));
00088     p = 0.1;
00089     EXPECT_FLOAT_EQ(p, probability(logodds(p)));
00090     p = 0.99;
00091     EXPECT_FLOAT_EQ(p, probability(logodds(p)));
00092 
00093     float l = 0;
00094     EXPECT_FLOAT_EQ(l, logodds(probability(l)));
00095     l = -4;
00096     EXPECT_FLOAT_EQ(l, logodds(probability(l)));
00097     l = 2;
00098     EXPECT_FLOAT_EQ(l, logodds(probability(l)));
00099 
00100 
00101     OcTree tree (0.05);
00102     tree.setProbHit(0.7);
00103     tree.setProbMiss(0.4);
00104 
00105     point3d origin (0.01f, 0.01f, 0.02f);
00106     point3d point_on_surface (2.01f,0.01f,0.01f);
00107   
00108     for (int i=0; i<360; i++) {    
00109       for (int j=0; j<360; j++) {
00110         EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface));
00111         point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
00112       }
00113       point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
00114     }
00115     EXPECT_TRUE (tree.writeBinary("sphere_rays.bt"));
00116     EXPECT_EQ ((int) tree.size(), 50615);
00117   
00118   // ------------------------------------------------------------
00119   // ray casting is now in "test_raycasting.cpp"
00120 
00121   // ------------------------------------------------------------
00122   // insert scan test
00123   // insert graph node test
00124   // write graph test
00125   } else if (test_name == "InsertScan") {
00126     Pointcloud* measurement = new Pointcloud();
00127   
00128     point3d origin (0.01f, 0.01f, 0.02f);
00129     point3d point_on_surface (2.01f, 0.01f, 0.01f);
00130   
00131     for (int i=0; i<360; i++) {
00132       for (int j=0; j<360; j++) {
00133         point3d p = origin+point_on_surface;
00134         measurement->push_back(p);
00135         point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
00136       }
00137       point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
00138     }
00139   
00140     OcTree tree (0.05);
00141     tree.insertPointCloud(*measurement, origin);
00142     EXPECT_EQ (tree.size(), 53959);
00143 
00144     ScanGraph* graph = new ScanGraph();
00145     Pose6D node_pose (origin.x(), origin.y(), origin.z(),0.0f,0.0f,0.0f);
00146     graph->addNode(measurement, node_pose);
00147     EXPECT_TRUE (graph->writeBinary("test.graph"));
00148     delete graph;
00149   // ------------------------------------------------------------
00150   // graph read file test
00151   } else if (test_name == "ReadGraph") {
00152     ScanGraph graph;
00153     EXPECT_TRUE (graph.readBinary("test.graph"));
00154   // ------------------------------------------------------------
00155 
00156   } else if (test_name == "StampedTree") {
00157     OcTreeStamped stamped_tree (0.05);
00158     // fill tree
00159     for (int x=-20; x<20; x++) 
00160       for (int y=-20; y<20; y++) 
00161         for (int z=-20; z<20; z++) {
00162           point3d p ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
00163           stamped_tree.updateNode(p, true); // integrate 'occupied' measurement 
00164         }
00165     // test if update times set
00166     point3d query (0.1f, 0.1f, 0.1f);
00167     OcTreeNodeStamped* result = stamped_tree.search (query);
00168     EXPECT_TRUE (result);
00169     unsigned int tree_time = stamped_tree.getLastUpdateTime();
00170     unsigned int node_time = result->getTimestamp();
00171     std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl;
00172     EXPECT_TRUE (tree_time > 0);
00173     #ifdef _WIN32
00174       Sleep(1000);
00175     #else
00176       sleep(1);
00177     #endif
00178     stamped_tree.integrateMissNoTime(result);  // reduce occupancy, no time update
00179     std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl;
00180     EXPECT_EQ  (node_time, result->getTimestamp()); // node time updated?
00181     point3d query2 = point3d  (0.1f, 0.1f, 0.3f);
00182     stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement
00183     OcTreeNodeStamped* result2 = stamped_tree.search (query2);
00184     EXPECT_TRUE (result2);
00185     result = stamped_tree.search (query);
00186     EXPECT_TRUE (result);
00187     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()
00188         << "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl;
00189     EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated
00190     EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime());
00191   // ------------------------------------------------------------
00192   } else if (test_name == "OcTreeKey") {
00193     OcTree tree (0.05);  
00194     point3d p(0.0,0.0,0.0);
00195     OcTreeKey key;
00196     tree.coordToKeyChecked(p, key);
00197     point3d p_inv = tree.keyToCoord(key);
00198     EXPECT_FLOAT_EQ (0.025, p_inv.x());
00199     EXPECT_FLOAT_EQ (0.025, p_inv.y());
00200     EXPECT_FLOAT_EQ (0.025, p_inv.z());
00201 
00202   // ------------------------------------------------------------
00203   } else {
00204     std::cerr << "Invalid test name specified: " << test_name << std::endl;
00205     return 1;
00206 
00207   }
00208 
00209   std::cerr << "Test successful.\n";
00210   return 0;
00211 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Jun 6 2019 17:31:45