Sample Pointclouds before Publishing

README.md

@@ -1,5 +1,7 @@
 <img src="https://github.com/ethz-asl/maplab/wiki/logos/maplab_new.png" width="500">
 
+
+
 *Ubuntu 14.04+ROS indigo*, *Ubuntu 16.04+ROS kinetic* and *Ubuntu 18.04+ROS melodic*: [![Build Status](https://jenkins.asl.ethz.ch/buildStatus/icon?job=maplab_nightly)](https://jenkins.asl.ethz.ch/job/maplab_nightly) 
 [![Documentation Status](https://jenkins.asl.ethz.ch/buildStatus/icon?job=maplab_docs&subject=docs)](https://jenkins.asl.ethz.ch/job/maplab_docs)
 

backend/map-resources/include/map-resources/resource-conversion.h

@@ -128,6 +128,11 @@ void resizePointCloud(
  const size_t size, const bool has_color, const bool has_normals,
  const bool has_scalar, const bool has_labels, PointCloudType* point_cloud);
 
+uint32_t getPointStep(
+ const bool has_color, const bool /*has_normals*/, const bool has_scalar,
+ const bool has_labels);
+
+
 } // namespace backend
 
 #include "map-resources/resource-conversion-inl.h"

backend/map-resources/src/resource-conversion.cc

@@ -744,6 +744,24 @@ void resizePointCloud(
  CHECK_EQ(hasLabelInformation(*point_cloud), has_labels);
 }
 
+uint32_t getPointStep(
+ const bool has_color, const bool /*has_normals*/, const bool has_scalar,
+ const bool has_labels) {
+ uint32_t offset = 0;
+
+ offset = 4u * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
+ if (has_color) {
+ offset += 4u * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
+ }
+ if (has_scalar) {
+ offset += 4u * sizeOfPointField(sensor_msgs::PointField::FLOAT32);
+ }
+ if (has_labels) {
+ offset += 4 * sizeOfPointField(sensor_msgs::PointField::UINT32);
+ }
+ return offset;
+}
+
 void createCameraWithoutDistortion(
  const aslam::Camera& camera,
  aslam::Camera::Ptr* camera_without_distortion) {

visualization/include/visualization/point-cloud-filter.h

@@ -0,0 +1,69 @@
+#ifndef VISUALIZATION_POINT_CLOUD_FILTER_H_
+#define VISUALIZATION_POINT_CLOUD_FILTER_H_
+
+#include <fstream>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <map-resources/resource-conversion.h>
+#include <pcl/common/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+namespace visualization {
+
+template <typename T>
+using PclPointCloudPtr = typename boost::shared_ptr<pcl::PointCloud<T>>;
+
+template <typename T_input, typename T_output>
+static void applyRandomDownSamplingFilter(
+ const T_input& cloud_in, const size_t& n_points_to_keep,
+ T_output* cloud_filtered) {
+ CHECK_NOTNULL(cloud_filtered);
+ CHECK_GE(cloud_in.size(), n_points_to_keep);
+ const bool input_has_normals = backend::hasNormalsInformation(cloud_in);
+ const bool input_has_scalars = backend::hasScalarInformation(cloud_in);
+ const bool input_has_color = backend::hasColorInformation(cloud_in);
+ const bool input_has_labels = backend::hasLabelInformation(cloud_in);
+
+ backend::resizePointCloud(
+ n_points_to_keep, input_has_color, input_has_normals, input_has_scalars,
+ input_has_labels, cloud_filtered);
+
+ const bool output_has_scalars =
+ backend::hasScalarInformation(*cloud_filtered);
+ const bool output_has_color = backend::hasColorInformation(*cloud_filtered);
+ const bool output_has_labels = backend::hasLabelInformation(*cloud_filtered);
+
+ std::vector<size_t> sampling_indices(cloud_in.size());
+ std::iota(sampling_indices.begin(), sampling_indices.end(), 0u);
+ std::random_shuffle(sampling_indices.begin(), sampling_indices.end());
+ for (size_t idx = 0u; idx < n_points_to_keep; ++idx) {
+ Eigen::Vector3d point_C;
+ backend::getPointFromPointCloud(cloud_in, sampling_indices[idx], &point_C);
+ backend::addPointToPointCloud(point_C, idx, cloud_filtered);
+
+ if (input_has_color && output_has_color) {
+ resources::RgbaColor color;
+ backend::getColorFromPointCloud(cloud_in, sampling_indices[idx], &color);
+ backend::addColorToPointCloud(color, idx, cloud_filtered);
+ }
+
+ if (input_has_scalars && output_has_scalars) {
+ float scalar;
+ backend::getScalarFromPointCloud(
+ cloud_in, sampling_indices[idx], &scalar);
+ backend::addScalarToPointCloud(scalar, idx, cloud_filtered);
+ }
+
+ if (input_has_labels && output_has_labels) {
+ uint32_t label;
+ backend::getLabelFromPointCloud(cloud_in, sampling_indices[idx], &label);
+ backend::addLabelToPointCloud(label, idx, cloud_filtered);
+ }
+ }
+}
+
+} // namespace visualization
+
+#endif // VISUALIZATION_POINT_CLOUD_FILTER_H_

visualization/src/resource-visualization.cc

@@ -1,8 +1,5 @@
 #include "visualization/resource-visualization.h"
 
-#include <chrono>
-#include <thread>
-
 #include <Eigen/Dense>
 #include <aslam/cameras/camera-factory.h>
 #include <aslam/cameras/camera-pinhole.h>
@@ -11,14 +8,17 @@
 #include <aslam/common/time.h>
 #include <aslam/frames/visual-frame.h>
 #include <aslam/frames/visual-nframe.h>
+#include <chrono>
 #include <depth-integration/depth-integration.h>
 #include <glog/logging.h>
 #include <maplab-common/progress-bar.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
+#include <thread>
 #include <vi-map/vertex.h>
 #include <vi-map/vi-map.h>
 #include <visualization/common-rviz-visualization.h>
+#include <visualization/point-cloud-filter.h>
 #include <visualization_msgs/MarkerArray.h>
 #include <voxblox/core/common.h>
 
@@ -65,6 +65,23 @@ DEFINE_bool(
 
 namespace visualization {
 
+template <typename T_input>
+static void convertAndSubsampleIfNecessary(
+ const T_input& cloud_in, sensor_msgs::PointCloud2* ros_point_cloud) {
+ const uint32_t point_step = backend::getPointStep(
+ cloud_in.hasColor(), cloud_in.hasNormals(), cloud_in.hasScalars(),
+ cloud_in.hasLabels());
+ static const size_t kMargin = 1000u;
+ static const size_t kMaxRosMessageSize = 1000000000u;
+ static const size_t kMaxDataSize = kMaxRosMessageSize - kMargin;
+ if (cloud_in.size() * point_step > kMaxDataSize) {
+ const int n_max_points = kMaxDataSize / point_step;
+ applyRandomDownSamplingFilter(cloud_in, n_max_points, ros_point_cloud);
+ } else {
+ backend::convertPointCloudType(cloud_in, ros_point_cloud);
+ }
+}
+
 bool visualizeCvMatResources(
  const vi_map::VIMap& map, backend::ResourceType type) {
  CHECK_GT(FLAGS_vis_resource_visualization_frequency, 0.0);
@@ -279,7 +296,7 @@ void visualizeReprojectedDepthResource(
  }
 
  sensor_msgs::PointCloud2 ros_point_cloud_G;
- backend::convertPointCloudType(points_G, &ros_point_cloud_G);
+ convertAndSubsampleIfNecessary(points_G, &ros_point_cloud_G);
 
  publishPointCloudInGlobalFrame(
  "" /*topic prefix*/, &ros_point_cloud_G);
@@ -304,7 +321,7 @@ void visualizeReprojectedDepthResource(
 
  // Publish accumulated point cloud in global frame.
  sensor_msgs::PointCloud2 ros_point_cloud_G;
- backend::convertPointCloudType(accumulated_point_cloud_G, &ros_point_cloud_G);
+ convertAndSubsampleIfNecessary(accumulated_point_cloud_G, &ros_point_cloud_G);
  publishPointCloudInGlobalFrame("" /*topic prefix*/, &ros_point_cloud_G);
 
  // Only continue if we want to export the accumulated point cloud to file.
@@ -412,7 +429,7 @@ void visualizeReprojectedDepthResourcePerRobot(
 
  // Publish accumulated point cloud in global frame.
  sensor_msgs::PointCloud2 ros_point_cloud_G;
- backend::convertPointCloudType(
+ convertAndSubsampleIfNecessary(
  accumulated_point_cloud_G, &ros_point_cloud_G);
  publishPointCloudInGlobalFrame(
  robot_name /*topic prefix*/, &ros_point_cloud_G);