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
This commit is contained in:
Vincent Rabaud
parent
ae5a7968da
commit
6cebfa088a
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
82
cmake/FindEigen.cmake
Normal file
82
cmake/FindEigen.cmake
Normal file
|
|
@ -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()
|
||||
|
||||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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) ;
|
||||
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<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]);
|
||||
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<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
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user