✨ add eigen find.cmake

CMakeLists.txt

@@ -26,11 +26,12 @@ else()
     message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
 endif()
 
+LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
 # Find Eigen3 Library
 # Since it is header-only and it won't be linked,
 # we have to explicitly add the include directories.
-find_package(Eigen3 3.2.10 REQUIRED)
+find_package(Eigen3 3.2 REQUIRED)
 include_directories(${EIGEN3_INCLUDE_DIR})
 
 ## Find catkin macros and libraries
@@ -74,4 +75,4 @@ add_executable(gps_conversion_node
 target_link_libraries(gps_conversion_node
         ${catkin_LIBRARIES}
         ${EIGEN3_LIBS}
-        )
+        )

cmake_modules/FindEigen3.cmake

@@ -0,0 +1,87 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <[email protected]>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <[email protected]>
+# Copyright (c) 2009 Benoit Jacob <[email protected]>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+  # specific additional paths for some OS
+  if (WIN32)
+    set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include")
+  endif(WIN32)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${EIGEN_ADDITIONAL_SEARCH_PATHS}
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
+

gps_conversion.cpp

@@ -17,7 +17,6 @@ external_info::gps_conversion::gps_conversion(ros::NodeHandle& nh_pub, ros::Node
     else
         rot_imu2cam << 0, -1, 0, 0, 0, -1, 1, 0, 0;
 
-//    rot_cam2imu = rot_imu2cam.transpose();
     q_imu2cam = rot_imu2cam;
 
     gps_pub_ = nh_pub.advertise<geometry_msgs::PoseWithCovarianceStamped>(pose_pub_topic, 100);
@@ -46,12 +45,10 @@ void external_info::gps_conversion::gps_callback(const geometry_msgs::PoseWithCo
 
     Eigen::Isometry3d cam_frame_pose = Eigen::Isometry3d::Identity();
 
-    cam_frame_pose = rot_imu2cam * gps_frame_rot_0_inverse * pose2enu ;  //pose(k) to enu, enu to pose(0), pose(0) to cam(0)
+//    cam_frame_pose = rot_imu2cam * gps_frame_rot_0_inverse * pose2enu ;  //pose(k) to enu, enu to pose(0), pose(0) to cam(0)
 
     cam_frame_pose = pose2enu * gps_frame_rot_0_inverse * q_imu2cam;
 
-//            Eigen::Quaterniond cam_frame_rot = gps_frame_rot * gps_frame_rot_0_inverse * q_imu2cam;
-
     if(frame_num_ == 0){
 
         off_v = cam_frame_pose.translation();

include/gps_conversion.h

@@ -34,7 +34,7 @@ namespace external_info
 
         Eigen::Quaterniond gps_frame_rot_0_inverse, q_imu2cam;
 
-        Eigen::Matrix3d rot_imu2cam, rot_cam2imu;
+        Eigen::Matrix3d rot_imu2cam;
 
         Eigen::Vector3d off_v, off_g;