ROS下使用PCL/kinfu

原创 于 2014-08-02 17:28:00 发布 · 6.6k 阅读 · 5 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#ros #pcl #cuda #gpu #kinfu

本文档描述了如何在ROS环境下编译和运行PCL（Point Cloud Library）的Kinfu程序，用于实现Kinect融合。作者在尝试使用ROS的pcl17包时发现相关代码缺失，于是决定从PCL源码中提取并改编kinfu的代码。通过详细步骤，包括创建ros_kinfu package、编辑package.xml文件和编译过程，最终展示了如何在ROS下运行kinect fusion程序。

摘要生成于 C知道 ，由 DeepSeek-R1 满血版支持， 前往体验 >

       最近想做机器人平台上的kinect fusion，在网上找了各种ROS下面kinfu相关的资料，大多残缺不全，唯一给了我灵感的这个网址：http://wiki.ros.org/pcl17，本以为自己找到了完美的解决方案，结果我却奇迹般地发现pcl17中kinfu相关的代码已经不存在。无奈，既然不能用ros下pcl17相关的代码，那我们就使用并改写https://github.com/PointCloudLibrary/pcl中的代码吧，既然pcl已经开源了kinfu的相关代码，那么把它移到ros下也不会是一件非常困难的事。

       在开始之前，向大家介绍一下我的软件平台，我的PC机上的OS版本为Ubuntu 12.04 LTS 64bit，ROS的版本为groovy。虽然没在其他平台上测试，不过理论上不会有太大差别。

  

编译PCL/kinfu

关于如何在ubuntu下编译PCL/kinfu，请参考我的上一篇博客“Ubuntu下编译PCL/KinFu”。
网址：http://blog.csdn.net/l_h2010/article/details/38239709

创建ros_kinfu package

创建一个package

假设你的catkin工作目录为~/catkin_ws
cd ~/catkin_ws/src
catkin_create_pkg kinfu


编辑package.xml文件

cd kinfu
编辑package.xml文件，内容如下： 


  kinfu
  0.0.0
  The kinfu package

  hao

  TODO
 

  catkin

  roscpp
  pcl
  std_msgs
  sensor_msgs
  tf
  image_transport
  dynamic_reconfigure

  message_runtime
  roscpp
  pcl
  std_msgs
  sensor_msgs
  tf
  image_transport
  dynamic_reconfigure
  roscpp


编辑CMakeLists.txt文件

编辑CMakeLists.txt文件，内容如下：
 

cmake_minimum_required(VERSION 2.8.3)
project(kinfu)

find_package(catkin REQUIRED
    COMPONENTS message_generation roscpp std_srvs std_msgs sensor_msgs dynamic_reconfigure tf image_transport)

#include_directories(${PCL_INCLUDE_DIRS})

find_package(Eigen REQUIRED)
include_directories(${EIGEN_INCLUDE_DIRS})
add_definitions(${EIGEN_DEFINITIONS})

find_package(VTK REQUIRED)
include_directories(${VTK_INCLUDE_DIRS})

#find_package(PCL REQUIRED)
#include_directories(${PCL_INCLUDE_DIRS})
#link_directories(${PCL_LIBRARY_DIRS})
#add_definitions(${PCL_DEFINITIONS})

set(PCL_INCLUDE_DIRS "/usr/local/include/pcl-1.7")
set(PCL_LIBRARY_DIRS "/usr/local/lib")

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})

include_directories("/usr/include/ni/")

#add dynamic reconfigure api
#generate_dynamic_reconfigure_options(cfg/dyn_config.cfg)
#include_directories(include cfg/cpp)

#### Cuda 
find_package(CUDA)

if (CUDA_FOUND) 
    message(" * CUDA ${CUDA_VERSION} was found")
    include(FindCUDA)
    include_directories(${CUDA_INCLUDE_DIRS})
else(CUDA_FOUND)
    message(" * CUDA is not found")
    message(FATAL_ERROR "Not all CUDA libraries are found")
endif(CUDA_FOUND) 

catkin_package(
   CATKIN_DEPENDS message_runtime std_msgs sensor_msgs dynamic_reconfigure
)

add_executable(kinfuLS src/kinfuLS_app.cpp src/evaluation.cpp)
#add_dependencies(kinfuLS ${PROJECT_NAME}_gencfg)

#target_link_libraries(kinfuLS boost_system boost_signals)
target_link_libraries(kinfuLS ${catkin_LIBRARIES})

target_link_libraries(kinfuLS ${CUDA_LIBRARIES})
#target_link_libraries(kinfuLS ${PCL_LIBRARY_DIRS})

target_link_libraries(kinfuLS pcl_gpu_kinfu_large_scale pcl_common pcl_io pcl_visualization pcl_octree pcl_gpu_containers vtkCommon vtkFiltering OpenNI vtkRendering)


添加并编写ros_kinfu相关代码

cd src
将pcl-trunk/gpu/kinfu_large_scale/tools目录下的color_handler.h，evaluation.h，evaluation.cpp，kinfuLS_app.cpp文件拷贝到当前目录。
修改kinfuLS_app.cpp，修改后的内容如下： 

#define _CRT_SECURE_NO_DEPRECATE

#include 

#include 

#include 

#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include "color_handler.h"
#include "evaluation.h"

#include 

#include 
#include 
#include 
#include "sensor_msgs/fill_image.h"
#include 

#include 
#include 
#include 
#include 
#include 
#include 


#ifdef HAVE_OPENCV  
#include 
#include 
#endif
typedef pcl::ScopeTime ScopeTimeT;

#include 

using namespace sensor_msgs;
using namespace message_filters;

using namespace std;
using namespace pcl;
using namespace Eigen;

using namespace pcl::gpu::kinfuLS;

using pcl::gpu::DeviceArray;
using pcl::gpu::DeviceArray2D;
using pcl::gpu::PtrStepSz;

namespace pc = pcl::console;

namespace pcl
{
namespace gpu
{
namespace kinfuLS
{
void paint3DView (const KinfuTracker::View& rgb24, KinfuTracker::View& view, float colors_weight = 0.5f);
void mergePointNormal (const DeviceArray& cloud, const DeviceArray& normals, DeviceArray& output);
}
}
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

vector getPcdFilesInDir(const string& directory)
{
    namespace fs = boost::filesystem;
    fs::path dir(directory);

    std::cout << "path: " << directory << std::endl;
    if (directory.empty() || !fs::exists(dir) || !fs::is_directory(dir))
        PCL_THROW_EXCEPTION (pcl::IOException, "No valid PCD directory given!\n");

    vector result;
    fs::directory_iterator pos(dir);
    fs::directory_iterator end;

    for(; pos != end ; ++pos)
        if (fs::is_regular_file(pos->status()) )
            if (fs::extension(*pos) == ".pcd")
            {
#if BOOST_FILESYSTEM_VERSION == 3
                result.push_back (pos->path ().string ());
#else
                result.push_back (pos->path ());
#endif
                cout << "added: " << result.back() << endl;
            }

    return result;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

struct SampledScopeTime : public StopWatch
{          
    enum { EACH = 33 };
    SampledScopeTime(int& time_ms) : time_ms_(time_ms) {}
    ~SampledScopeTime()
    {
        static int i_ = 0;
        static boost::posix_time::ptime starttime_ = boost::posix_time::microsec_clock::local_time();
        time_ms_ += getTime ();
        if (i_ % EACH == 0 && i_)
        {
            boost::posix_time::ptime endtime_ = boost::posix_time::microsec_clock::local_time();
            cout << "Average frame time = " << time_ms_ / EACH << "ms ( " << 1000.f * EACH / time_ms_ << "fps )"
                 << "( real: " << 1000.f * EACH / (endtime_-starttime_).total_milliseconds() << "fps )"  << endl;
            time_ms_ = 0;
            starttime_ = endtime_;
        }
        ++i_;
    }
private:    
    int& time_ms_;
};

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
setViewerPose (visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
    Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
    Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;
    Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);
    viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
            look_at_vector[0], look_at_vector[1], look_at_vector[2],
            up_vector[0], up_vector[1], up_vector[2]);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

Eigen::Affine3f 
getViewerPose (visualization::PCLVisualizer& viewer)
{
    Eigen::Affine3f pose = viewer.getViewerPose();
    Eigen::Matrix3f rotation = pose.linear();

    Matrix3f axis_reorder;
    axis_reorder << 0,  0,  1,
            -1,  0,  0,
            0, -1,  0;

    rotation = rotation * axis_reorder;
    pose.linear() = rotation;
    return pose;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

template void
writeCloudFile (int format, const CloudT& cloud);

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
writePolygonMeshFile (int format, const pcl::PolygonMesh& mesh);


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

template
typename PointCloud::Ptr merge(const PointCloud& points, const PointCloud& colors)
{    
    typename PointCloud::Ptr merged_ptr(new PointCloud());

    pcl::copyPointCloud (points, *merged_ptr);
    for (size_t i = 0; i < colors.size (); ++i)
        merged_ptr->points[i].rgba = colors.points[i].rgba;

    return merged_ptr;
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

boost::shared_ptr convertToMesh(const DeviceArray& triangles)
{ 
    if (triangles.empty())
        return boost::shared_ptr();

    pcl::PointCloud cloud;
    cloud.width  = (int)triangles.size();
    cloud.height = 1;
    triangles.download(cloud.points);

    boost::shared_ptr mesh_ptr( new pcl::PolygonMesh() );
    pcl::toPCLPointCloud2(cloud, mesh_ptr->cloud);

    mesh_ptr->polygons.resize (triangles.size() / 3);
    for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
    {
        pcl::Vertices v;
        v.vertices.push_back(i*3+0);
        v.vertices.push_back(i*3+2);
        v.vertices.push_back(i*3+1);
        mesh_ptr->polygons[i] = v;
    }
    return mesh_ptr;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

struct CurrentFrameCloudView
{
    CurrentFrameCloudView() : cloud_device_ (480, 640), cloud_viewer_ ("Frame Cloud Viewer")
    {
        cloud_ptr_ = PointCloud::Ptr (new PointCloud);

        cloud_viewer_.setBackgroundColor (0, 0, 0.15);
        cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 1);
        cloud_viewer_.addCoordinateSystem (1.0, "global");
        cloud_viewer_.initCameraParameters ();
        cloud_viewer_.setPosition (0, 500);
        cloud_viewer_.setSize (640, 480);
        cloud_viewer_.setCameraClipDistances (0.01, 10.01);
    }

    void
    show (const KinfuTracker& kinfu)
    {
        kinfu.getLastFrameCloud (cloud_device_);

        int c;
        cloud_device_.download (cloud_ptr_->points, c);
        cloud_ptr_->width = cloud_device_.cols ();
        cloud_ptr_->height = cloud_device_.rows ();
        cloud_ptr_->is_dense = false;

        cloud_viewer_.removeAllPointClouds ();
        cloud_viewer_.addPointCloud(cloud_ptr_);
        cloud_viewer_.spinOnce ();
    }

    void
    setViewerPose (const Eigen::Affine3f& viewer_pose) {
        ::setViewerPose (cloud_viewer_, viewer_pose);
    }

    PointCloud::Ptr cloud_ptr_;
    DeviceArray2D cloud_device_;
    visualization::PCLVisualizer cloud_viewer_;
};

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

struct ImageView
{
    ImageView() : paint_image_ (false), accumulate_views_ (false)
    {
        viewerScene_.setWindowTitle ("View3D from ray tracing");
        viewerScene_.setPosition (0, 0);
        //viewerDepth_.setWindowTitle ("Kinect Depth stream");
        //viewerDepth_.setPosition (640, 0);
        //viewerColor_.setWindowTitle ("Kinect RGB stream");
    }

    void
    showScene (KinfuTracker& kinfu, const PtrStepSz& rgb24, bool registration, Eigen::Affine3f* pose_ptr = 0)
    {
        if (pose_ptr)
        {
            raycaster_ptr_->run ( kinfu.volume (), *pose_ptr, kinfu.getCyclicalBufferStructure () ); //says in cmake it does not know it
            raycaster_ptr_->generateSceneView(view_device_);
        }
        else
        {
            kinfu.getImage (view_device_);
        }

        if (paint_image_ && registration && !pose_ptr)
        {
            colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
            paint3DView (colors_device_, view_device_);
最低0.47元/天 解锁文章
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值