use the new bullet and eigen conventions

git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@38342 eb33c2ac-9c88-4c90-87e0-44a10359b0c3

CMakeLists.txt

@@ -1,10 +1,15 @@
 cmake_minimum_required(VERSION 2.4.6)
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
+
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 set(ROS_BUILD_TYPE Debug)
 rosbuild_init()
 
 rosbuild_add_boost_directories()
 
+find_package(Eigen REQUIRED)
+include_directories(${EIGEN_INCLUDE_DIRS})
+
 rosbuild_add_library(laser_geometry src/laser_geometry.cpp )
 rosbuild_link_boost(laser_geometry thread)
 

cmake/FindEigen.cmake

@@ -0,0 +1,82 @@
+# - 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
+#
+#  EIGEN_FOUND - system has eigen lib with correct version
+#  EIGEN_INCLUDE_DIR - the eigen include directory
+#  EIGEN_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen_FIND_VERSION)
+  if(NOT Eigen_FIND_VERSION_MAJOR)
+    set(Eigen_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen_FIND_VERSION_MAJOR)
+  if(NOT Eigen_FIND_VERSION_MINOR)
+    set(Eigen_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen_FIND_VERSION_MINOR)
+  if(NOT Eigen_FIND_VERSION_PATCH)
+    set(Eigen_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen_FIND_VERSION_PATCH)
+
+  set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}")
+endif(NOT Eigen_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN_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(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
+  if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
+    set(EIGEN_VERSION_OK FALSE)
+  else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
+    set(EIGEN_VERSION_OK TRUE)
+  endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
+
+  if(NOT EIGEN_VERSION_OK)
+
+    message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, "
+                   "but at least version ${Eigen_FIND_VERSION} is required")
+  endif(NOT EIGEN_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN_INCLUDE_DIRS)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN_FOUND ${EIGEN_VERSION_OK})
+
+else ()
+
+  find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK)
+
+  mark_as_advanced(EIGEN_INCLUDE_DIR)
+  SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.")
+
+endif()
+

manifest.xml

@@ -15,11 +15,16 @@ for the skew resulting from moving robots or tilting laser scanners.
 <depend package="roscpp"/>
 <depend package="tf"/>
 <depend package="angles"/>
-<depend package="eigen"/>
+<depend package="common_rosdeps" />
+<rosdep name="eigen" />
 <export>
 <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_geometry"/>
 </export>
   <platform os="ubuntu" version="9.04"/>
   <platform os="ubuntu" version="9.10"/>
   <platform os="ubuntu" version="10.04"/>
+  <platform os="ubuntu" version="10.10"/>
+  <platform os="ubuntu" version="11.04"/>
+  <platform os="ubuntu" version="11.10"/>
+  <platform os="ubuntu" version="12.04"/>
 </package>

src/laser_geometry.cpp

@@ -242,17 +242,17 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
       uint32_t pt_index = cloud_out.channels[index_channel_idx].values[i];
 
       // Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
-      btScalar ratio = pt_index / ( (double) scan_in.ranges.size() - 1.0) ;
+      tfScalar ratio = pt_index / ( (double) scan_in.ranges.size() - 1.0) ;
 
       //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
 
       //Interpolate translation
-      btVector3 v (0, 0, 0);
+      tf::Vector3 v (0, 0, 0);
       v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
       cur_transform.setOrigin(v) ;
 
       //Interpolate rotation
-      btQuaternion q1, q2 ;
+      tf::Quaternion q1, q2 ;
       start_transform.getBasis().getRotation(q1) ;
       end_transform.getBasis().getRotation(q2) ;
 
@@ -260,8 +260,8 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
       cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
 
       // Apply the transform to the current point
-      btVector3 pointIn(cloud_out.points[i].x, cloud_out.points[i].y, cloud_out.points[i].z) ;
+      tf::Vector3 pointIn(cloud_out.points[i].x, cloud_out.points[i].y, cloud_out.points[i].z) ;
-      btVector3 pointOut = cur_transform * pointIn ;
+      tf::Vector3 pointOut = cur_transform * pointIn ;
 
       // Copy transformed point into cloud
       cloud_out.points[i].x  = pointOut.x();
@@ -559,24 +559,24 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
       memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
       
       // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
-      btScalar ratio = pt_index * ranges_norm;
+      tfScalar ratio = pt_index * ranges_norm;
 
       //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
       // Interpolate translation
-      btVector3 v (0, 0, 0);
+      tf::Vector3 v (0, 0, 0);
       v.setInterpolate3 (start_transform.getOrigin (), end_transform.getOrigin (), ratio);
       cur_transform.setOrigin (v);
 
       // Interpolate rotation
-      btQuaternion q1, q2;
+      tf::Quaternion q1, q2;
       start_transform.getBasis ().getRotation (q1);
       end_transform.getBasis ().getRotation (q2);
 
       // Compute the slerp-ed rotation
       cur_transform.setRotation (slerp (q1, q2 , ratio));
 
-      btVector3 point_in (pstep[0], pstep[1], pstep[2]);
+      tf::Vector3 point_in (pstep[0], pstep[1], pstep[2]);
-      btVector3 point_out = cur_transform * point_in;
+      tf::Vector3 point_out = cur_transform * point_in;
 
       // Copy transformed point into cloud
       pstep[0] = point_out.x ();
@@ -587,7 +587,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
       if(has_viewpoint)
       {
         float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset];
-        point_in = btVector3 (vpstep[0], vpstep[1], vpstep[2]);
+        point_in = tf::Vector3 (vpstep[0], vpstep[1], vpstep[2]);
         point_out = cur_transform * point_in;
 
         // Copy transformed point into cloud