added CTest environment

use: make test

added math tests
added octomap tests

CMakeLists.txt

@@ -1,6 +1,8 @@
 CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 PROJECT( octomap-distribution )
 
+ENABLE_TESTING()  # enable CTest environment
+
 ADD_SUBDIRECTORY( octomap )
 ADD_SUBDIRECTORY( octovis )
 

octomap/CMakeLists.txt

@@ -1,6 +1,8 @@
 CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 PROJECT( octomap )
 
+ENABLE_TESTING()
+
 # version (e.g. for packaging)
 set(OCTOMAP_MAJOR_VERSION 1)
 set(OCTOMAP_MINOR_VERSION 2)
@@ -137,4 +139,3 @@ MESSAGE (STATUS "Compile octomap using: make")
 MESSAGE (STATUS "Install octomap using: make install")
 MESSAGE (    "    (be sure to set the correct CMAKE_INSTALL_PREFIX before)")
 MESSAGE (STATUS "Compile API-documentation using: make docs\n")
-MESSAGE (STATUS "")

octomap/src/octomap/testing/CMakeLists.txt

@@ -1,5 +1,4 @@
 
-
 ADD_EXECUTABLE(test_raycasting test_raycasting.cpp)
 TARGET_LINK_LIBRARIES(test_raycasting octomap)
 
@@ -12,3 +11,23 @@ TARGET_LINK_LIBRARIES(test_scans octomap)
 ADD_EXECUTABLE(test_lut test_lut.cpp)
 TARGET_LINK_LIBRARIES(test_lut octomap)
 
+ADD_EXECUTABLE(test_math test_math.cpp)
+TARGET_LINK_LIBRARIES(test_math octomap)
+
+ADD_EXECUTABLE(test_octomap test_octomap.cpp)
+TARGET_LINK_LIBRARIES(test_octomap octomap)
+
+ADD_TEST(
+  NAME TestMath
+  COMMAND test_math
+)
+
+ADD_TEST(
+  NAME TestOctomap
+  COMMAND test_octomap
+)
+
+# ADD_TEST(
+#   NAME TestRayCast 
+#   COMMAND test_raycasting
+# )

octomap/src/octomap/testing/test_math.cpp

@@ -0,0 +1,65 @@
+#include <stdio.h>
+#include <octomap/octomap.h>
+#include "testing.h"
+
+using namespace std;
+using namespace octomath;
+
+int main(int argc, char** argv) {
+
+  // test constructors
+  Vector3* twos = new Vector3();        
+  Vector3* ones = new Vector3(1,1,1);    
+  for (int i=0;i<3;i++) {
+    (*twos)(i) = 2;
+  }
+
+  // test basic operations
+  Vector3 subtraction = *twos - *ones;
+  Vector3 addition = *twos + *ones;
+  Vector3 multiplication = *twos * 2.;
+
+  for (int i=0;i<3;i++) {
+    EXPECT_FLOAT_EQ (subtraction(i), 1.);
+    EXPECT_FLOAT_EQ (addition(i), 3.);
+    EXPECT_FLOAT_EQ (multiplication(i), 4.);
+  }
+
+  // copy constructor
+  Vector3 rotation =  *ones;
+
+  // rotation
+  rotation.rotate_IP (M_PI, 1., 0.1);
+  EXPECT_FLOAT_EQ (rotation.x(), 1.2750367);
+  EXPECT_FLOAT_EQ (rotation.y(), -1.1329513);
+  EXPECT_FLOAT_EQ (rotation.z(), 0.30116868);
+
+
+  // constructors  
+  Pose6D a (1. ,0.1 ,0.1 , 0., 0.1, M_PI/4.);
+  Pose6D b ();
+
+  Vector3 trans(1., 0.1, 0.1);
+  Quaternion rot(0., 0.1, M_PI/4.);
+  Pose6D c(trans, rot);
+
+  // comparator
+  EXPECT_TRUE ( a == c);
+  // toEuler
+  EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.);
+
+  // transform
+  Vector3 t = c.transform (trans);
+  EXPECT_FLOAT_EQ (t.x() , 1.6399229);
+  EXPECT_FLOAT_EQ (t.y() , 0.8813442);
+  EXPECT_FLOAT_EQ (t.z() , 0.099667005);
+
+  // inverse transform
+  Pose6D c_inv = c.inv();
+  Vector3 t2 = c_inv.transform (t);
+  EXPECT_FLOAT_EQ (t2.x() , trans.x());
+  EXPECT_FLOAT_EQ (t2.y() , trans.y());
+  EXPECT_FLOAT_EQ (t2.z() , trans.z());
+
+  return 0;
+}

octomap/src/octomap/testing/test_octomap.cpp

@@ -0,0 +1,112 @@
+#include <stdio.h>
+#include <octomap/octomap.h>
+#include <octomap/math/Utils.h>
+#include "testing.h"
+
+using namespace std;
+using namespace octomap;
+using namespace octomath;
+
+int main(int argc, char** argv) {
+
+  {
+    OcTree tree (0.05);
+    tree.setProbHit(0.7);
+    tree.setProbMiss(0.4);
+
+    point3d origin (0.01, 0.01, 0.02);
+    point3d point_on_surface (2.01,0.01,0.01);
+
+    for (int i=0; i<360; i++) {    
+      for (int j=0; j<360; j++) {
+        EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface));
+        point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
+      }
+      point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
+    }
+
+    tree.writeBinary("sphere.bt");
+
+    OcTree sampled_surface (0.05);  
+    point3d direction = point3d (1.0,0.0,0.0);
+    point3d obstacle(0,0,0);
+    unsigned int hits (0);
+    unsigned int misses (0);
+    double mean_dist(0);
+
+    for (int i=0; i<360; i++) {    
+      for (int j=0; j<360; j++) {
+        if (!tree.castRay(origin, direction, obstacle, true, 3.)) {
+          misses++;
+        }
+        else {
+          hits++;
+          mean_dist += (obstacle - origin).norm ();
+          sampled_surface.updateNode(obstacle, true);
+        }
+        direction.rotate_IP (0,0,DEG2RAD(1.));
+      }
+      direction.rotate_IP (0,DEG2RAD(1.),0);
+    }
+
+    mean_dist /= (double) hits;
+    //   std::cout << " hits / misses: " << hits  << " / " << misses << std::endl;
+    //   std::cout << " mean obstacle dist: " << mean_dist << std::endl;
+
+    //   cout << "writing sampled_surface.bt: " << flush;
+    sampled_surface.writeBinary("sampled_surface.bt");
+
+    EXPECT_EQ ((int) hits, 129416);
+    EXPECT_EQ ((int) misses,  184);
+    EXPECT_NEAR(mean_dist, 2., 0.1);
+    printf("expected: 129416, got: %d\n ", hits);
+  }
+
+  // insert scan test
+  // insert graph node test
+  {
+    Pointcloud* measurement = new Pointcloud();
+
+    point3d origin (0.01, 0.01, 0.02);
+    point3d point_on_surface (2.01,0.01,0.01);
+
+    for (int i=0; i<360; i++) {    
+      for (int j=0; j<360; j++) {
+        point3d p = origin+point_on_surface;
+        measurement->push_back(p);
+        point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
+      }
+      point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
+    }
+
+    OcTree tree (0.05);
+    tree.insertScan(*measurement, origin);
+    EXPECT_EQ ((int) tree.size(), 54076);
+
+    ScanGraph* graph = new ScanGraph();
+    Pose6D node_pose (0.01, 0.01, 0.02, 0,0,0);
+    graph->addNode(measurement, node_pose);
+    graph->writeBinary("test.graph");
+    delete graph;
+  }
+
+  // tree read file test
+  {
+    OcTree tree (0.05);  
+    tree.readBinary("sphere.bt");
+  }
+
+  // graph read file test
+  {
+    ScanGraph graph;
+    graph.readBinary("test.graph");
+  }
+
+  // re-read written file (ensure that file is readable)
+  {
+    OcTree tree (0.05);  
+    tree.readBinary("sphere.bt");
+  }
+
+  return 0;
+}

octomap/src/octomap/testing/test_raycasting.cpp

@@ -93,4 +93,5 @@ int main(int argc, char** argv) {
   cout << "writing to beams.bt..." << endl;
   single_beams.writeBinary("beams.bt");
 
+  return 0;
 }

octomap/src/octomap/testing/testing.h

@@ -0,0 +1,29 @@
+#include <math.h>
+#include <stdlib.h>
+
+// this is mimicing gtest expressions
+
+bool EXPECT_EQ (int a, int b) {
+  if (a == b) return true;
+  fprintf(stderr, "test failed: %d != %d\n", a, b);
+  exit(-1);
+}
+
+bool EXPECT_FLOAT_EQ (float a, float b) {
+  if (fabs(a-b) <= 1e-5) return true;
+  fprintf(stderr, "test failed: %f != %f\n", a, b);
+  exit(-1);
+}
+
+bool EXPECT_TRUE (bool b) {
+  if (b) return true;
+  fprintf(stderr, "test failed\n");
+  exit(-1);
+}
+
+bool EXPECT_NEAR (float a, float b, float prec) {
+  if (fabs(a-b) <= prec) return true;
+  fprintf(stderr, "test failed: |%f - %f| > %f\n", a, b, prec);
+  exit(-1);
+}
+