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 }