Fawkes API  Fawkes Development Version
utils.h
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