Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * utils.h - General PCL utilities 00004 * 00005 * Created: Tue Nov 08 17:50:07 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #ifndef __LIBS_PCL_UTILS_UTILS_H_ 00023 #define __LIBS_PCL_UTILS_UTILS_H_ 00024 00025 #include <pcl/point_cloud.h> 00026 00027 namespace fawkes { 00028 namespace pcl_utils { 00029 #if 0 /* just to make Emacs auto-indent happy */ 00030 } 00031 } 00032 #endif 00033 00034 /** Set time of a point cloud from a fawkes::Time instance. 00035 * This uses the fawkes::PointCloudTimestamp struct to set the time in the PCL 00036 * timestamp field (if non-ROS PCL is used). 00037 * @param cloud cloud of which to set the time 00038 * @param time time to use 00039 */ 00040 template <typename PointT> 00041 inline void 00042 set_time(fawkes::RefPtr<pcl::PointCloud<PointT> > &cloud, const fawkes::Time &time) 00043 { 00044 #if HAVE_ROS_PCL 00045 cloud->header.stamp.sec = time.get_sec(); 00046 cloud->header.stamp.nsec = time.get_usec() * 1000; 00047 #else 00048 fawkes::PointCloudTimestamp pclts; 00049 pclts.time.sec = time.get_sec(); 00050 pclts.time.usec = time.get_usec(); 00051 cloud->header.stamp = pclts.timestamp; 00052 #endif 00053 } 00054 00055 00056 /** Get time of a point cloud as a fawkes::Time instance. 00057 * This uses the fawkes::PointCloudTimestamp struct to set the time in the PCL 00058 * timestamp field (if non-ROS PCL is used). 00059 * @param cloud cloud of which to get the time 00060 * @param time upon return contains the timestamp of the cloud 00061 */ 00062 template <typename PointT> 00063 inline void 00064 get_time(const fawkes::RefPtr<const pcl::PointCloud<PointT> > &cloud, fawkes::Time &time) 00065 { 00066 #if HAVE_ROS_PCL 00067 time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000); 00068 #else 00069 fawkes::PointCloudTimestamp pclts; 00070 pclts.timestamp = cloud->header.stamp; 00071 time.set_time(pclts.time.sec, time.get_usec()); 00072 #endif 00073 } 00074 00075 00076 /** Get time of a point cloud as a fawkes::Time instance. 00077 * This uses the fawkes::PointCloudTimestamp struct to set the time in the PCL 00078 * timestamp field (if non-ROS PCL is used). 00079 * @param cloud cloud of which to get the time 00080 * @param time upon return contains the timestamp of the cloud 00081 */ 00082 template <typename PointT> 00083 inline void 00084 get_time(const fawkes::RefPtr<pcl::PointCloud<PointT> > &cloud, fawkes::Time &time) 00085 { 00086 #if HAVE_ROS_PCL 00087 time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000); 00088 #else 00089 fawkes::PointCloudTimestamp pclts; 00090 pclts.timestamp = cloud->header.stamp; 00091 time.set_time(pclts.time.sec, time.get_usec()); 00092 #endif 00093 } 00094 00095 00096 /** Copy time from one point cloud to another. 00097 * @param from point cloud to copy time from 00098 * @param to point cloud to copy time to 00099 */ 00100 template <typename PointT1, typename PointT2> 00101 inline void 00102 copy_time(fawkes::RefPtr<const pcl::PointCloud<PointT1> > &from, 00103 fawkes::RefPtr<pcl::PointCloud<PointT2> > &to) 00104 { 00105 to->header.stamp = from->header.stamp; 00106 } 00107 00108 00109 /** Helper struct to avoid deletion of PointClouds. 00110 * The input point cloud is accessible using a RefPtr. Since the PCL 00111 * expectes Boost shared_ptr, we need to create such a shared pointer. 00112 * But destruction of this would cause the deletion of the point cloud, 00113 * which we do not want. Therefore, we provide this helper deleter 00114 * that causes the PointCloud *not* to be deleted on reset. 00115 */ 00116 struct PointCloudNonDeleter { 00117 /** Delete operator that does nothing. 00118 * @param t object to destroy 00119 */ 00120 template<typename T> void operator()(T*t) {} 00121 }; 00122 00123 template <typename PointT> 00124 typename pcl::PointCloud<PointT>::Ptr 00125 cloudptr_from_refptr(fawkes::RefPtr<pcl::PointCloud<PointT> > &in) 00126 { 00127 return 00128 boost::shared_ptr<pcl::PointCloud<PointT> >(*in, PointCloudNonDeleter()); 00129 } 00130 00131 00132 template <typename PointT> 00133 typename pcl::PointCloud<PointT>::ConstPtr 00134 cloudptr_from_refptr(fawkes::RefPtr<const pcl::PointCloud<PointT> > &in) 00135 { 00136 return 00137 boost::shared_ptr<const pcl::PointCloud<PointT> >(*in, PointCloudNonDeleter()); 00138 } 00139 00140 } // end namespace pclutils 00141 } // end namespace fawkes 00142 00143 #endif