Merge branch 'gtest_motion_2d' into 'master'

gtest cannot find Eigen/Dense

I am having this bug when compiling gtest_motion_2d.cpp:

```
In file included from /home/jsola/dev/wolf/test/../src/ceres_wrapper/ceres_manager.h:5:0,
                 from /home/jsola/dev/wolf/test/gtest_motion_2d.cpp:15:
/usr/local/include/ceres/jet.h:165:22: fatal error: Eigen/Core: No such file or directory
 #include "Eigen/Core"
```

I added `eigen_include_dirs` and `ceres_include_dirs` in the `test/CMakeFiles` file, but no luck:
```cmake
## Added these two include_dirs: ######################
#
# Include Eigen
INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
#
# Include Ceres
IF(Ceres_FOUND)
    INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
ENDIF(Ceres_FOUND)
#
## but still gtest_motion_2d does not work ################
```

@artivis could you please have a look?

See merge request !103

CMakeLists.txt

@@ -130,14 +130,10 @@ ENDIF(UNIX)
 #############
 ## Testing ##
 #############
-
+#
 if(BUILD_TESTS)
-
-# Enables testing for this directory and below.
-# Note that ctest expects to find a test file in the build directory root.
-# Therefore, this command should be in the source directory root.
-enable_testing()
-
-add_subdirectory(${PROJECT_SOURCE_DIR}/test)
-
+    # Enables testing for this directory and below.
+    # Note that ctest expects to find a test file in the build directory root.
+    # Therefore, this command should be in the source directory root.
+    enable_testing()
 endif()

src/CMakeLists.txt

@@ -19,7 +19,6 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 option(BUILD_EXAMPLES "Build examples" ON)
 
 
-
 # Does this has any other interest
 # but for the examples ?
 #IF(BUILD_EXAMPLES)
@@ -481,6 +480,18 @@ export(PACKAGE ${PROJECT_NAME})
 
 IF(BUILD_EXAMPLES)
   #Build examples & tests
-  MESSAGE("Building examples and tests.")
+  MESSAGE("Building examples.")
   ADD_SUBDIRECTORY(examples)
 ENDIF(BUILD_EXAMPLES)
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTS)
+    
+    MESSAGE("Building tests.")
+    add_subdirectory(test)
+
+endif()
+

src/examples/CMakeLists.txt

@@ -21,8 +21,8 @@ IF(Ceres_FOUND)
     ADD_EXECUTABLE(test_processor_odom_3D test_processor_odom_3D.cpp)
     TARGET_LINK_LIBRARIES(test_processor_odom_3D ${PROJECT_NAME})
 
-    ADD_EXECUTABLE(test_motion_2d test_motion_2d.cpp)
-    TARGET_LINK_LIBRARIES(test_motion_2d ${PROJECT_NAME})
+#    ADD_EXECUTABLE(test_motion_2d test_motion_2d.cpp)
+#    TARGET_LINK_LIBRARIES(test_motion_2d ${PROJECT_NAME})
 ENDIF(Ceres_FOUND)
 
 

src/processor_odom_2D.h

@@ -105,7 +105,6 @@ inline void ProcessorOdom2D::data2delta(const Eigen::VectorXs& _data, const Eige
 
     delta_cov_ = J * _data_cov * J.transpose();
 
-    WOLF_TRACE("");
     //std::cout << "data      :" << _data.transpose() << std::endl;
     //std::cout << "data cov  :" << std::endl << _data_cov << std::endl;
     //std::cout << "delta     :" << delta_.transpose() << std::endl;

test/CMakeLists.txt → src/test/CMakeLists.txt

 # Retrieve googletest from github & compile
-add_subdirectory(${PROJECT_SOURCE_DIR}/test/gtest)
+add_subdirectory(gtest)
 
 # Include config.h directory at first.
 include_directories(${PROJECT_BINARY_DIR}/conf/)
@@ -7,6 +7,27 @@ include_directories(${PROJECT_BINARY_DIR}/conf/)
 # Include third party directory.
 include_directories(${PROJECT_SOURCE_DIR}/third_party/spdlog/include/)
 
+
+
+## Added these two include_dirs: ######################
+#
+#CMAKE modules
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
+MESSAGE(STATUS ${CMAKE_MODULE_PATH})
+
+# Include Eigen
+FIND_PACKAGE(Eigen 3 REQUIRED)
+INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
+#
+# Include Ceres
+FIND_PACKAGE(Ceres QUIET) #Ceres is not required
+IF(Ceres_FOUND)
+    INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
+ENDIF(Ceres_FOUND)
+#
+## and now gtest_motion_2d works ######################
+
+
 # Include gtest directory.
 include_directories(${GTEST_INCLUDE_DIRS})
 
@@ -28,6 +49,10 @@ target_link_libraries(gtest_example ${PROJECT_NAME})      #
 wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
 target_link_libraries(gtest_local_param ${PROJECT_NAME})
 
+# ProcessorMotion in 2D
+wolf_add_gtest(gtest_motion_2d gtest_motion_2d.cpp)
+target_link_libraries(gtest_motion_2d ${PROJECT_NAME})
+
 # MotionBuffer class test
 wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
 target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})

src/examples/test_motion_2d.cpp → src/test/gtest_motion_2d.cpp

@@ -5,14 +5,16 @@
  *      \author: jsola
  */
 
+#include "utils_gtest.h"
+
 // Classes under test
-#include "processor_odom_2D.h"
+#include "../processor_odom_2D.h"
 
 // Wolf includes
-#include "capture_fix.h"
-#include "state_block.h"
-#include "wolf.h"
-#include "ceres_wrapper/ceres_manager.h"
+#include "../capture_fix.h"
+#include "../state_block.h"
+#include "../wolf.h"
+#include "../ceres_wrapper/ceres_manager.h"
 
 // STL includes
 #include <map>
@@ -24,7 +26,7 @@
 #include <iostream>
 #include <iomanip>      // std::setprecision
 
-int main()
+TEST(ProcessorMotion, Motion2D)
 {
     std::cout << std::setprecision(3);
 
@@ -103,6 +105,7 @@ int main()
 
     for (int i = 0; i <= 20; i++)
     {
+        WOLF_TRACE("");
         // Processor
         odom2d_ptr->process(cap_ptr);
 
@@ -143,39 +146,42 @@ int main()
         integrated_x_vector.push_back(integrated_x);
         integrated_covariance_vector.push_back(integrated_covariance);
 
-        if ((odom2d_ptr->getCurrentState() - integrated_x).norm() > Constants::EPS)
-        {
-            std::cout << "----------- PROCESSOR:" << std::endl;
-            std::cout << "State(" << (t - t0) << ") : " << odom2d_ptr->getCurrentState().transpose() << std::endl;
-            std::cout << "Covariance(" << (t - t0) << ") : " << std::endl
-                      << odom2d_ptr->getBuffer().get().back().delta_integr_cov_ << std::endl;
-            std::cout << "REFERENCE:" << std::endl;
-            std::cout << "State(" << (t - t0) << ") : " << integrated_x.transpose() << std::endl;
-            std::cout << "Covariance(" << (t - t0) << ") : " << std::endl << integrated_delta_covariance << std::endl;
-            std::cout << "ERROR:" << std::endl;
-            std::cout << "State error(" << (t - t0) << ") : " << odom2d_ptr->getCurrentState().transpose() - integrated_x.transpose() << std::endl;
-            std::cout << "Covariance error(" << (t - t0) << ") : " << std::endl
-                      << odom2d_ptr->getBuffer().get().back().delta_integr_cov_ - integrated_delta_covariance << std::endl;
-
-            throw std::runtime_error("Integrated state different from reference.");
-        }
-
-        if ((odom2d_ptr->getBuffer().get().back().delta_integr_cov_ - integrated_delta_covariance).array().abs().maxCoeff() > Constants::EPS)
-        {
-            std::cout << "----------- PROCESSOR:" << std::endl;
-            std::cout << "State(" << (t - t0) << ") : " << odom2d_ptr->getCurrentState().transpose() << std::endl;
-            std::cout << "Covariance(" << (t - t0) << ") : " << std::endl
-                      << odom2d_ptr->getBuffer().get().back().delta_integr_cov_ << std::endl;
-            std::cout << "REFERENCE:" << std::endl;
-            std::cout << "State(" << (t - t0) << ") : " << integrated_x.transpose() << std::endl;
-            std::cout << "Covariance(" << (t - t0) << ") : " << std::endl << integrated_delta_covariance << std::endl;
-            std::cout << "ERROR:" << std::endl;
-            std::cout << "State error(" << (t - t0) << ") : " << odom2d_ptr->getCurrentState().transpose() - integrated_x.transpose() << std::endl;
-            std::cout << "Covariance error(" << (t - t0) << ") : " << std::endl
-                      << odom2d_ptr->getBuffer().get().back().delta_integr_cov_ - integrated_delta_covariance << std::endl;
-//            throw std::runtime_error("Integrated covariance different from reference.");
-            std::cout << "TEST COVARIANCE CHECK ------> ERROR: Integrated covariance different from reference." << std::endl;
-        }
+//        if ((odom2d_ptr->getCurrentState() - integrated_x).norm() > Constants::EPS)
+//        {
+//            std::cout << "----------- PROCESSOR:" << std::endl;
+//            std::cout << "State(" << (t - t0) << ") : " << odom2d_ptr->getCurrentState().transpose() << std::endl;
+//            std::cout << "Covariance(" << (t - t0) << ") : " << std::endl
+//                      << odom2d_ptr->getBuffer().get().back().delta_integr_cov_ << std::endl;
+//            std::cout << "REFERENCE:" << std::endl;
+//            std::cout << "State(" << (t - t0) << ") : " << integrated_x.transpose() << std::endl;
+//            std::cout << "Covariance(" << (t - t0) << ") : " << std::endl << integrated_delta_covariance << std::endl;
+//            std::cout << "ERROR:" << std::endl;
+//            std::cout << "State error(" << (t - t0) << ") : " << odom2d_ptr->getCurrentState().transpose() - integrated_x.transpose() << std::endl;
+//            std::cout << "Covariance error(" << (t - t0) << ") : " << std::endl
+//                      << odom2d_ptr->getBuffer().get().back().delta_integr_cov_ - integrated_delta_covariance << std::endl;
+//
+//            std::cout << "TEST DELTA CHECK ------> ERROR: Integrated state different from reference." << std::endl;
+//        }
+        EXPECT_TRUE((odom2d_ptr->getCurrentState() - integrated_x).isMuchSmallerThan(1.0,Constants::EPS));
+
+//        if ((odom2d_ptr->getBuffer().get().back().delta_integr_cov_ - integrated_delta_covariance).array().abs().maxCoeff() > Constants::EPS)
+//        {
+//            std::cout << "----------- PROCESSOR:" << std::endl;
+//            std::cout << "State(" << (t - t0) << ") : " << odom2d_ptr->getCurrentState().transpose() << std::endl;
+//            std::cout << "Covariance(" << (t - t0) << ") : " << std::endl
+//                      << odom2d_ptr->getBuffer().get().back().delta_integr_cov_ << std::endl;
+//            std::cout << "REFERENCE:" << std::endl;
+//            std::cout << "State(" << (t - t0) << ") : " << integrated_x.transpose() << std::endl;
+//            std::cout << "Covariance(" << (t - t0) << ") : " << std::endl << integrated_delta_covariance << std::endl;
+//            std::cout << "ERROR:" << std::endl;
+//            std::cout << "State error(" << (t - t0) << ") : " << odom2d_ptr->getCurrentState().transpose() - integrated_x.transpose() << std::endl;
+//            std::cout << "Covariance error(" << (t - t0) << ") : " << std::endl
+//                      << odom2d_ptr->getBuffer().get().back().delta_integr_cov_ - integrated_delta_covariance << std::endl;
+//
+//            std::cout << "TEST COVARIANCE CHECK ------> ERROR: Integrated covariance different from reference." << std::endl;
+//        }
+        EXPECT_TRUE((odom2d_ptr->getBuffer().get().back().delta_integr_cov_ - integrated_delta_covariance).isMuchSmallerThan(1.0,Constants::EPS));
+
         // Timestamp
         t += dt;
         cap_ptr->setTimeStamp(t);
@@ -232,18 +238,17 @@ int main()
     //std::cout << summary.FullReport() << std::endl;
     ceres_manager_ptr->computeCovariances(ALL_MARGINALS);
 
-    if ((problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[12]).array().abs().maxCoeff() > 1e-10)
-    {
-        std::cout << "After solving the problem, covariance of new keyframe:" << std::endl;
-        std::cout << "WOLF:" << std::endl << problem_ptr->getFrameCovariance(new_keyframe_ptr) << std::endl;
-        std::cout << "REFERENCE:" << std::endl << integrated_covariance_vector[12] << std::endl;
-        std::cout << "ERROR:" << std::endl << problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[12] << std::endl;
-//        throw std::runtime_error("Integrated covariance different from reference.");
-        std::cout << "1st TEST COVARIANCE CHECK ------> ERROR!: Integrated covariance different from reference." << std::endl;
-
-    }
-    else
-        std::cout << "1st TEST COVARIANCE CHECK ------> OK!" << std::endl;
+//    if ((problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[12]).array().abs().maxCoeff() > 1e-10)
+//    {
+//        std::cout << "After solving the problem, covariance of new keyframe:" << std::endl;
+//        std::cout << "WOLF:" << std::endl << problem_ptr->getFrameCovariance(new_keyframe_ptr) << std::endl;
+//        std::cout << "REFERENCE:" << std::endl << integrated_covariance_vector[12] << std::endl;
+//        std::cout << "ERROR:" << std::endl << problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[12] << std::endl;
+////        throw std::runtime_error("Integrated covariance different from reference.");
+//        std::cout << "1st TEST COVARIANCE CHECK ------> ERROR!: Integrated covariance different from reference." << std::endl;
+//
+//    }
+    EXPECT_TRUE((problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[12]).isMuchSmallerThan(1.0, Constants::EPS));
 
 
     // second split as non-exact timestamp
@@ -258,18 +263,17 @@ int main()
     //std::cout << summary.FullReport() << std::endl;
     ceres_manager_ptr->computeCovariances(ALL_MARGINALS);
 
-    if ((problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[5]).array().abs().maxCoeff() > 1e-10)
-    {
-        std::cout << "After solving the problem, covariance of new keyframe:" << std::endl;
-        std::cout << "WOLF:" << std::endl << problem_ptr->getFrameCovariance(new_keyframe_ptr) << std::endl;
-        std::cout << "REFERENCE:" << std::endl << integrated_covariance_vector[5] << std::endl;
-        std::cout << "ERROR:" << std::endl << problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[5] << std::endl;
-//        throw std::runtime_error("Integrated covariance different from reference.");
-        std::cout << "2nd TEST COVARIANCE CHECK ------> ERROR!: Integrated covariance different from reference." << std::endl;
-
-    }
-    else
-        std::cout << "2nd TEST COVARIANCE CHECK ------> OK!" << std::endl;
+//    if ((problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[5]).array().abs().maxCoeff() > 1e-10)
+//    {
+//        std::cout << "After solving the problem, covariance of new keyframe:" << std::endl;
+//        std::cout << "WOLF:" << std::endl << problem_ptr->getFrameCovariance(new_keyframe_ptr) << std::endl;
+//        std::cout << "REFERENCE:" << std::endl << integrated_covariance_vector[5] << std::endl;
+//        std::cout << "ERROR:" << std::endl << problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[5] << std::endl;
+////        throw std::runtime_error("Integrated covariance different from reference.");
+//        std::cout << "2nd TEST COVARIANCE CHECK ------> ERROR!: Integrated covariance different from reference." << std::endl;
+//
+//    }
+    EXPECT_TRUE((problem_ptr->getFrameCovariance(new_keyframe_ptr) - integrated_covariance_vector[5]).isMuchSmallerThan(1.0, Constants::EPS));
 
 
     std::cout << "All in one row:            < ";
@@ -285,6 +289,12 @@ int main()
     std::cout << "ceres manager deleted" << std::endl;
     problem_ptr.reset();
     std::cout << "problem deleted" << std::endl;
+}
 
-    return 0;
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
 }
+

test/gtest_motion_buffer.cpp → src/test/gtest_motion_buffer.cpp

@@ -7,9 +7,9 @@
 
 
 #include "utils_gtest.h"
-#include "../src/logging.h"
+#include "../logging.h"
 
-#include "../src/motion_buffer.h"
+#include "../motion_buffer.h"
 
 #include <iostream>
 

test/gtest_odom_3D.cpp → src/test/gtest_odom_3D.cpp

@@ -9,11 +9,11 @@
 
 
 #include "utils_gtest.h"
-#include "../src/logging.h"
+#include "../logging.h"
 
-#include "../src/processor_odom_3D.h"
+#include "../processor_odom_3D.h"
 
-#include "../src/wolf.h"
+#include "../wolf.h"
 
 #include <iostream>
 

test/gtest_time_stamp.cpp → src/test/gtest_time_stamp.cpp

 #include "utils_gtest.h"
-#include "../src/time_stamp.h"
+#include "../time_stamp.h"
 
 #include <thread>
 

test/gtest_trajectory.cpp → src/test/gtest_trajectory.cpp

@@ -9,9 +9,9 @@
 #include "utils_gtest.h"
 #include "../src/logging.h"
 
-#include "../src/problem.h"
-#include "../src/trajectory_base.h"
-#include "../src/frame_base.h"
+#include "../problem.h"
+#include "../trajectory_base.h"
+#include "../frame_base.h"
 
 #include <iostream>