From 6cebfa088aaf8a790dc2bddcb8f91a1ddb7e5105 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Tue, 13 Dec 2011 22:29:05 +0000 Subject: [PATCH] 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 | 5 +++ cmake/FindEigen.cmake | 82 ++++++++++++++++++++++++++++++++++++++++++ manifest.xml | 7 +++- src/laser_geometry.cpp | 22 ++++++------ 4 files changed, 104 insertions(+), 12 deletions(-) create mode 100644 cmake/FindEigen.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 8cd44b7..cc0e617 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/cmake/FindEigen.cmake b/cmake/FindEigen.cmake new file mode 100644 index 0000000..126f4a0 --- /dev/null +++ b/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, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# 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() + diff --git a/manifest.xml b/manifest.xml index f3faa14..29a1739 100644 --- a/manifest.xml +++ b/manifest.xml @@ -15,11 +15,16 @@ for the skew resulting from moving robots or tilting laser scanners. - + + + + + + diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index d8ede30..ac0bd13 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -242,17 +242,17 @@ const boost::numeric::ublas::matrix& 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& 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) ; - btVector3 pointOut = cur_transform * pointIn ; + tf::Vector3 pointIn(cloud_out.points[i].x, cloud_out.points[i].y, cloud_out.points[i].z) ; + 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& 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]); - btVector3 point_out = cur_transform * point_in; + tf::Vector3 point_in (pstep[0], pstep[1], pstep[2]); + 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& 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