diff --git a/CMakeLists.txt b/CMakeLists.txt index cebf1a6c..187ccea5 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/octomap/CMakeLists.txt b/octomap/CMakeLists.txt index 5c0b636f..81b9e5d4 100644 --- a/octomap/CMakeLists.txt +++ b/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 "") diff --git a/octomap/src/octomap/testing/CMakeLists.txt b/octomap/src/octomap/testing/CMakeLists.txt index a6b43ed1..8abc44b8 100644 --- a/octomap/src/octomap/testing/CMakeLists.txt +++ b/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 +# ) diff --git a/octomap/src/octomap/testing/test_math.cpp b/octomap/src/octomap/testing/test_math.cpp new file mode 100644 index 00000000..dd90cdf1 --- /dev/null +++ b/octomap/src/octomap/testing/test_math.cpp @@ -0,0 +1,65 @@ +#include +#include +#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; +} diff --git a/octomap/src/octomap/testing/test_octomap.cpp b/octomap/src/octomap/testing/test_octomap.cpp new file mode 100644 index 00000000..a09a7486 --- /dev/null +++ b/octomap/src/octomap/testing/test_octomap.cpp @@ -0,0 +1,112 @@ +#include +#include +#include +#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; +} diff --git a/octomap/src/octomap/testing/test_raycasting.cpp b/octomap/src/octomap/testing/test_raycasting.cpp index 1023449d..b3e28416 100644 --- a/octomap/src/octomap/testing/test_raycasting.cpp +++ b/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; } diff --git a/octomap/src/octomap/testing/testing.h b/octomap/src/octomap/testing/testing.h new file mode 100644 index 00000000..06fe12fe --- /dev/null +++ b/octomap/src/octomap/testing/testing.h @@ -0,0 +1,29 @@ +#include +#include + +// 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); +} +