surface_history_recorder.cpp
Go to the documentation of this file.
00001 #include "surface_perception/surface_history_recorder.h"
00002 
00003 #include <ctime>
00004 #include <map>
00005 
00006 #include "pcl/PointIndices.h"
00007 #include "pcl/filters/extract_indices.h"
00008 #include "pcl/point_cloud.h"
00009 #include "pcl/point_types.h"
00010 #include "ros/ros.h"
00011 
00012 typedef pcl::PointXYZRGB PointC;
00013 typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudC;
00014 
00015 namespace surface_perception {
00016 void SurfaceHistoryRecorder::Record(size_t id, const PointCloudC::Ptr& cloud,
00017                                     const pcl::PointIndices::Ptr& indices,
00018                                     size_t iteration) {
00019   Update(0, id, cloud, indices, iteration);
00020 }
00021 
00022 void SurfaceHistoryRecorder::Update(size_t old_id, size_t new_id,
00023                                     const PointCloudC::Ptr& cloud,
00024                                     const pcl::PointIndices::Ptr& indices,
00025                                     size_t iteration) {
00026   if (new_id == 0) {
00027     ROS_INFO("Warning: Update(...) doesn't update id to 0.");
00028     return;
00029   }
00030 
00031   PointCloudC::Ptr new_cloud(new PointCloudC);
00032   pcl::ExtractIndices<PointC> extract_indices;
00033   extract_indices.setInputCloud(cloud);
00034   extract_indices.setIndices(indices);
00035   extract_indices.filter(*new_cloud);
00036 
00037   std::map<size_t, PointCloudC::Ptr>::iterator iter =
00038       cloud_history_.find(old_id);
00039   if (iter != cloud_history_.end()) {
00040     *new_cloud += *(iter->second);
00041   }
00042 
00043   cloud_history_[new_id] = new_cloud;
00044   time_history_[new_id] = std::clock();
00045   iteration_history_[new_id] = iteration;
00046 }
00047 
00048 void SurfaceHistoryRecorder::GetCloudHistory(
00049     size_t id, const PointCloudC::Ptr output_cloud) const {
00050   std::map<size_t, PointCloudC::Ptr>::const_iterator iter =
00051       cloud_history_.find(id);
00052   if (iter != cloud_history_.end()) {
00053     *output_cloud = *(iter->second);
00054   }
00055 }
00056 
00057 void SurfaceHistoryRecorder::GetClock(size_t id, clock_t* clock_ptr) const {
00058   std::map<size_t, clock_t>::const_iterator iter = time_history_.find(id);
00059   if (iter != time_history_.end()) {
00060     *clock_ptr = iter->second;
00061   }
00062 }
00063 
00064 void SurfaceHistoryRecorder::GetIteration(size_t id,
00065                                           size_t* iteration_ptr) const {
00066   std::map<size_t, size_t>::const_iterator iter = iteration_history_.find(id);
00067   if (iter != iteration_history_.end()) {
00068     *iteration_ptr = iter->second;
00069   }
00070 }
00071 }  // namespace surface_perception


surface_perception
Author(s): Yu-Tang Peng
autogenerated on Thu Jun 6 2019 17:36:21