Add unit test for Pointcloud and ScanGraph IO (#131)

octomap/src/testing/CMakeLists.txt

@@ -39,6 +39,7 @@ ADD_TEST (NAME InsertScan         COMMAND unit_tests InsertScan     )
 ADD_TEST (NAME ReadGraph          COMMAND unit_tests ReadGraph      )
 ADD_TEST (NAME StampedTree        COMMAND unit_tests StampedTree    )
 ADD_TEST (NAME OcTreeKey          COMMAND unit_tests OcTreeKey      )
+ADD_TEST (NAME test_scans         COMMAND test_scans ${PROJECT_SOURCE_DIR}/share/data/spherical_scan.graph)
 ADD_TEST (NAME test_raycasting    COMMAND test_raycasting)
 ADD_TEST (NAME test_io            COMMAND test_io ${PROJECT_SOURCE_DIR}/share/data/geb079.bt)
 ADD_TEST (NAME test_pruning       COMMAND test_pruning )

octomap/src/testing/test_scans.cpp

@@ -2,41 +2,101 @@
 #include <stdio.h>
 #include <octomap/octomap.h>
 #include <octomap/math/Utils.h>
+#include "testing.h"
 
 using namespace std;
 using namespace octomap;
 
+void printUsage(char* self){
+  std::cerr << "\nUSAGE: " << self << " spherical_scan.graph  (reference file to compare, required)\n\n";
 
+  exit(1);
+}
 
 int main(int argc, char** argv) {
+  if (argc != 2){
+    printUsage(argv[0]);
+  }
 
+  std::string filename = std::string(argv[1]);
+
+  ScanGraph referenceGraph;
+  EXPECT_TRUE(referenceGraph.readBinary(filename));
+
+  // TODO: read in reference graph file
 
-  //##############################################################     
 
-  OcTree tree (0.05);  
+  //##############################################################     
 
-  point3d origin (0.01f, 0.01f, 0.02f);
   point3d point_on_surface (4.01f, 0.01f, 0.01f);
 
-  cout << "generating spherical scan at " << origin << " ..." << endl;
 
-  Pointcloud cloud;
+  Pointcloud* cloud = new Pointcloud();
 
-  for (int i=-100; i<101; i++) {
-    for (int j=-100; j<101; j++) {
+  for (int i=-50; i<51; i++) {
+    for (int j=-50; j<51; j++) {
       point3d rotated = point_on_surface;
       rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
-      cloud.push_back(rotated);
+      cloud->push_back(rotated);
     }
-  }  
-
-  // insert in global coordinates:
-  tree.insertPointCloud(cloud, origin);
-
-  cout << "done." << endl;
-  cout << "writing to spherical_scan.bt..." << endl;
-  tree.writeBinary("spherical_scan.bt");
-
-
+  }
+
+  pose6d origin(1.0, 0, -0.5, 0, 0, 0);
+
+  ScanGraph graph;
+  graph.addNode(cloud, origin); // graph assumes ownership of cloud!
+
+  {
+    std::cout << "Comparing ScanGraph with reference file at " << filename << std::endl;
+    EXPECT_TRUE(graph.size() == referenceGraph.size());
+    ScanNode* scanNode = *graph.begin();
+    ScanNode* refScanNode = *referenceGraph.begin();
+
+    EXPECT_EQ(scanNode->id, refScanNode->id);
+    EXPECT_EQ(scanNode->pose, refScanNode->pose);
+    EXPECT_EQ(scanNode->scan->size(), refScanNode->scan->size());
+
+    for (size_t i = 0; i < scanNode->scan->size(); ++i){
+      EXPECT_EQ((*scanNode->scan)[i], (*refScanNode->scan)[i]);
+    }  
+
+  }
+  // test reading and writing to file
+  {
+    std::cout << "Testing ScanGraph I/O" << std::endl;
+
+    EXPECT_TRUE(graph.writeBinary("spherical_scan_out.graph"));
+
+    ScanGraph reReadGraph;
+    EXPECT_TRUE(reReadGraph.readBinary("spherical_scan_out.graph"));
+
+    EXPECT_TRUE(graph.size() == reReadGraph.size());
+    EXPECT_EQ(reReadGraph.size(), 1);
+
+    ScanNode* scanNode = *graph.begin();
+    ScanNode* readScanNode = *reReadGraph.begin();
+
+    EXPECT_EQ(scanNode->id, readScanNode->id);
+    EXPECT_EQ(scanNode->pose, readScanNode->pose);
+    EXPECT_EQ(scanNode->scan->size(), readScanNode->scan->size());
+
+    for (size_t i = 0; i < scanNode->scan->size(); ++i){
+      EXPECT_EQ((*scanNode->scan)[i], (*readScanNode->scan)[i]);
+    }
+  }
+
+
+  // insert into OcTree  
+  {
+    OcTree tree (0.05);  
+
+    // insert in global coordinates:
+    tree.insertPointCloud(*cloud, origin.trans());
+
+    tree.writeBinary("spherical_scan.bt");
+  }
+
+  cout << "Test done." << endl;
+  exit(0);
 
 }

octomap/src/testing/unit_tests.cpp

@@ -149,6 +149,7 @@ int main(int argc, char** argv) {
   // ------------------------------------------------------------
   // graph read file test
   } else if (test_name == "ReadGraph") {
+    // not really meaningful, see better test in "test_scans.cpp"
     ScanGraph graph;
     EXPECT_TRUE (graph.readBinary("test.graph"));
   // ------------------------------------------------------------