Added random sample heatmapper.

rapid_perception/CMakeLists.txt

@@ -126,6 +126,7 @@ set(MY_RAPID_PERCEPTION_EXPORTED_LIBRARIES
   rapid_perception_pose_estimation_heat_mapper
   rapid_perception_fpfh_heat_mapper
   rapid_perception_template_matching_heat_mapper
+  rapid_perception_random_heat_mapper
   rapid_perception_pr2
   rapid_perception_rgbd
   rapid_perception_scene
@@ -305,6 +306,20 @@ target_link_libraries(rapid_perception_template_matching_heat_mapper
   rapid_perception_pose_estimation_heat_mapper
 )
 
+add_library(rapid_perception_random_heat_mapper
+  src/random_heat_mapper.cpp
+)
+add_dependencies(rapid_perception_random_heat_mapper
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  rapid_perception_pose_estimation_heat_mapper
+)
+target_link_libraries(rapid_perception_random_heat_mapper
+  ${PCL_LIBRARIES}
+  ${catkin_LIBRARIES}
+  rapid_perception_pose_estimation_heat_mapper
+)
+
 add_library(rapid_perception_pose_estimation
   src/pose_estimation.cpp
 )

rapid_perception/include/rapid_perception/random_heat_mapper.h

@@ -0,0 +1,32 @@
+#ifndef _RAPID_PERCEPTION_RANDOM_HEAT_MAPPER_H_
+#define _RAPID_PERCEPTION_RANDOM_HEAT_MAPPER_H_
+
+#include "Eigen/Core"
+#include "pcl/PointIndices.h"
+#include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
+
+#include "rapid_perception/pose_estimation_heat_mapper.h"
+
+namespace rapid {
+namespace perception {
+// A heat mapper that just gives equal weight to a random sample of the scene.
+class RandomHeatMapper : public PoseEstimationHeatMapper {
+ public:
+  RandomHeatMapper();
+  void Compute(pcl::PointIndicesPtr indices, Eigen::VectorXd* importances);
+  void set_scene(pcl::PointCloud<pcl::PointXYZRGB>::Ptr scene);
+  void set_object(pcl::PointCloud<pcl::PointXYZRGB>::Ptr object);
+  void set_sample_ratio(double val);
+  void set_max_samples(int val);
+
+ private:
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scene_;
+
+  double sample_ratio_;
+  int max_samples_;
+};
+}  // namespace perception
+}  // namespace rapid
+
+#endif  // _RAPID_PERCEPTION_RANDOM_HEAT_MAPPER_H_

rapid_perception/src/random_heat_mapper.cpp

@@ -0,0 +1,72 @@
+#include "rapid_perception/random_heat_mapper.h"
+
+#include <algorithm>
+#include <iostream>
+#include <math.h>
+#include <vector>
+
+#include "Eigen/Core"
+#include "pcl/PointIndices.h"
+#include "pcl/filters/extract_indices.h"
+#include "pcl/filters/random_sample.h"
+#include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
+
+#include "rapid_viz/publish.h"
+
+typedef pcl::PointXYZRGB PointC;
+typedef pcl::PointCloud<PointC> PointCloudC;
+using std::vector;
+
+namespace rapid {
+namespace perception {
+RandomHeatMapper::RandomHeatMapper()
+    : scene_(), sample_ratio_(0.05), max_samples_(1000) {}
+
+void RandomHeatMapper::Compute(pcl::PointIndicesPtr indices,
+                               Eigen::VectorXd* importances) {
+  indices->indices.clear();
+
+  int num_samples = static_cast<int>(round(sample_ratio_ * scene_->size()));
+  num_samples = std::min(num_samples, max_samples_);
+
+  pcl::RandomSample<PointC> random;
+  random.setSeed(0);
+  random.setSample(num_samples);
+  random.setInputCloud(scene_);
+  random.filter(indices->indices);
+
+  ROS_INFO("Randomly sampled %ld points", indices->indices.size());
+
+  importances->resize(num_samples);
+  importances->fill(1);
+
+  // Color point cloud for visualization
+  PointCloudC::Ptr working_scene(new PointCloudC);
+  *working_scene = *scene_;
+  for (size_t indices_i = 0; indices_i < indices->indices.size(); ++indices_i) {
+    int color = static_cast<int>(round(255 * (*importances)(indices_i)));
+    int index = indices->indices[indices_i];
+    working_scene->points[index].r = color;
+    working_scene->points[index].g = color;
+    working_scene->points[index].b = color;
+  }
+  PointCloudC::Ptr viz_cloud(new PointCloudC());
+  pcl::ExtractIndices<PointC> extract;
+  extract.setInputCloud(working_scene);
+  extract.setIndices(indices);
+  extract.filter(*viz_cloud);
+  viz::PublishCloud(heatmap_pub_, *viz_cloud);
+}
+
+void RandomHeatMapper::set_scene(PointCloudC::Ptr scene) { scene_ = scene; }
+
+void RandomHeatMapper::set_object(PointCloudC::Ptr object) {
+  // This function intentionally left empty.
+}
+
+void RandomHeatMapper::set_sample_ratio(double val) { sample_ratio_ = val; }
+
+void RandomHeatMapper::set_max_samples(int val) { max_samples_ = val; }
+}  // namespace perception
+}  // namespace rapid