Start on spherical projection

Author: john-maidbot

projected_point_cloud_transport/include/projected_point_cloud_transport/projected_publisher.hpp

@@ -37,6 +37,7 @@
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
+#include <point_cloud_interfaces/msg/compressed_point_cloud2.hpp>
 #include <point_cloud_transport/point_cloud_transport.hpp>
 
 #include <point_cloud_transport/simple_publisher_plugin.hpp>

projected_point_cloud_transport/src/projected_publisher.cpp

@@ -31,6 +31,9 @@
 
 #include <string>
 
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/point_cloud_iterator.hpp>
+
 #include <projected_point_cloud_transport/projected_publisher.hpp>
 
 namespace projected_point_cloud_transport
@@ -59,7 +62,39 @@ ProjectedPublisher::TypedEncodeResult ProjectedPublisher::encodeTyped(
 
   // TODO ([email protected]): Apply selected projection method
 
+  // SPHERICAL PROJECTION
+  double phi_resolution = 0.5; // radians
+  double theta_resolution = 0.5; // radians
+  int phi_bins = 2 * M_PI / phi_resolution;
+  int theta_bins = 2 * M_PI / rho_resolution;
+
+  cv::Mat spherical_image{phi_bins, theta_bins, CV_16UC1, cv::Scalar(0)};
+
+  // iterate over the pointcloud and convert to polar coordinates
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(raw, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(raw, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(raw, "z");
+
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    double x = *iter_x;
+    double y = *iter_y;
+    double z = *iter_z;
+
+    double rho = std::sqrt(x * x + y * y + z * z);
+    double phi = std::atan2(y, x);
+    double theta = std::acos(z / rho);
+
+    int phi_index = phi / phi_resolution;
+    int theta_index = theta / theta_resolution;
+
+    auto& cell = spherical_image.at<uint16_t>(phi_index, rho_index);
+    cell = std::min(cell, static_cast<uint16_t>(rho * 1000)); // mm resolution
+  }
+
   // TODO ([email protected]): Apply png compression
+  point_cloud_interfaces::msg::CompressPointCloud2 compressed;
+
+  cv::imencode(".png", spherical_image, compressed.compressed_data);
 
   compressed.width = raw.width;
   compressed.height = raw.height;

projected_point_cloud_transport/src/projected_subscriber.cpp

@@ -32,6 +32,8 @@
 #include <string>
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <sensor_msgs/point_cloud2_modifier.hpp>
 
 #include <projected_point_cloud_transport/projected_subscriber.hpp>
 
@@ -61,10 +63,49 @@ ProjectedSubscriber::DecodeResult ProjectedSubscriber::decodeTyped(
   // - spherical deprojection relative to some origin:
   // ---> viewpoint origin
 
-  // TODO ([email protected]): Apply selected deprojection method
+  // SPHERICAL PROJECTION
+  cv::Mat spherical_image       = cv::imdecode(cv::Mat(msg.compressed_data), cv::IMREAD_UNCHANGED);
+  int phi_bins = spherical_image.rows;
+  int theta_bins = spherical_image.cols;
+  double phi_resolution = phi_bins / 2.0 / M_PI; // radians
+  double theta_resolution = theta_bins / 2.0 / M_PI; // radians
 
-  result->width = msg.width;
-  result->height = msg.height;
+  sensor_msgs::msg::PointCloud2 cloud;
+  cloud.header = msg.header;
+
+  sensor_msgs::PointCloud2Modifier modifier(cloud);
+  modifier.setPointCloud2Fields(3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
+                                sensor_msgs::msg::PointField::FLOAT32, "z", 1,
+                                sensor_msgs::msg::PointField::FLOAT32);
+  modifier.resize(spherical_image.rows * spherical_image.cols);
+  sensor_msgs::PointCloud2Iterator<float> pcl_itr(cloud, "x");
+
+  size_t valid_points = 0;
+  // convert the spherical image to a point cloud
+  for(int row = 0; row < spherical_image.rows; row++)
+  {
+    for(int col = 0; col < spherical_image.cols; col++)
+    {
+      uint16_t cell = spherical_image.at<uint16_t>(row, col);
+      if(cell == 0)
+      {
+        continue;
+      }
+      // convert the spherical image pixel to a point cloud point
+      double rho = static_cast<double>(cell) / 1000.0; // meters
+      double phi = (row - phi_bins / 2.0) / phi_resolution;
+      double theta = (col - theta_bins / 2.0) / theta_resolution;
+      pcl_itr[0] = rho * std::sin(phi) * std::cos(theta);
+      pcl_itr[1] = rho * std::sin(phi) * std::sin(theta);
+      pcl_itr[2] = rho * std::cos(phi);
+      ++pcl_itr;
+      valid_points++;
+    }
+  }
+  modifier.resize(valid_points);
+
+  result->width = valid_points;
+  result->height = 1;
   result->row_step = msg.row_step;
   result->point_step = msg.point_step;
   result->is_bigendian = msg.is_bigendian;