最近想做机器人平台上的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文件，内容如下：
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)
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_); } int cols; view_device_.download (view_host_, cols); viewerScene_.showRGBImage (reinterpret_cast
(&view_host_[0]), view_device_.cols (), view_device_.rows ()); //viewerColor_.showRGBImage ((unsigned char*)&rgb24.data, rgb24.cols, rgb24.rows); #ifdef HAVE_OPENCV if (accumulate_views_) { views_.push_back (cv::Mat ()); cv::cvtColor (cv::Mat (480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back (), CV_RGB2GRAY); //cv::copy(cv::Mat(480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back()); } #endif } /*void showDepth (const PtrStepSz
& depth) { viewerDepth_.showShortImage (depth.data, depth.cols, depth.rows, 0, 5000, true); } void showGeneratedDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose) { raycaster_ptr_->run(kinfu.volume(), pose, kinfu.getCyclicalBufferStructure ()); raycaster_ptr_->generateDepthImage(generated_depth_); int c; vector
data; generated_depth_.download(data, c); viewerDepth_.showShortImage (&data[0], generated_depth_.cols(), generated_depth_.rows(), 0, 5000, true); }*/ void toggleImagePaint() { paint_image_ = !paint_image_; cout << "Paint image: " << (paint_image_ ? "On (requires registration mode)" : "Off") << endl; } bool paint_image_; bool accumulate_views_; visualization::ImageViewer viewerScene_; //view the raycasted model //visualization::ImageViewer viewerDepth_; //view the current depth map //visualization::ImageViewer viewerColor_; KinfuTracker::View view_device_; KinfuTracker::View colors_device_; vector
view_host_; RayCaster::Ptr raycaster_ptr_; K
最低0.47元/天 解锁文章
3561
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
