Builds

point_cloud_interfaces/CMakeLists.txt

@@ -3,17 +3,19 @@ project(point_cloud_interfaces)
 
 find_package(ament_cmake REQUIRED)
 find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 
 set(msg_files
   "msg/CompressedPointCloud2.msg"
+  "msg/ProjectedPointCloud.msg"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   ${msg_files}
-  DEPENDENCIES sensor_msgs std_msgs builtin_interfaces
+  DEPENDENCIES geometry_msgs sensor_msgs std_msgs builtin_interfaces
 )
 
 if(BUILD_TESTING)

point_cloud_interfaces/msg/ProjectedPointCloud.msg

@@ -17,7 +17,7 @@ bool was_dense
 uint8[] compressed_data
 
 # This is the point/orientation that acts as a reference for the pointcloud compression
-geometry_msgs/Pose view_point;
+geometry_msgs/Pose view_point
 
 # TODO: How to maintain fields data?
 # sensor_msgs/PointField[] fields

point_cloud_interfaces/package.xml

@@ -16,6 +16,7 @@
   <build_depend>builtin_interfaces</build_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
+  <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
 

projected_point_cloud_transport/CMakeLists.txt

@@ -5,27 +5,33 @@ set(CMAKE_CXX_STANDARD 17)
 project(projected_point_cloud_transport)
 
 find_package(ament_cmake REQUIRED)
+find_package(OpenCV REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(point_cloud_interfaces REQUIRED)
 find_package(point_cloud_transport REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
 
 set(dependencies
+  OpenCV
   pluginlib
   point_cloud_interfaces
   point_cloud_transport
   rclcpp
+  sensor_msgs
 )
 
 
-include_directories(include)
+include_directories(
+  include
+  ${OpenCV_INCLUDE_DIRS}
+)
 
 add_library(${PROJECT_NAME}
   SHARED
+  src/manifest.cpp
   src/projected_publisher.cpp
   src/projected_subscriber.cpp
-  src/projected_cpp.cpp
-  src/manifest.cpp
 )
 
 ament_target_dependencies(${PROJECT_NAME} ${dependencies})

projected_point_cloud_transport/include/projected_point_cloud_transport/projected_publisher.hpp

@@ -35,13 +35,14 @@
 #include <memory>
 #include <string>
 
+#include <opencv2/core.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
-#include <point_cloud_interfaces/msg/compressed_point_cloud2.hpp>
+#include <point_cloud_interfaces/msg/projected_point_cloud.hpp>
 #include <point_cloud_transport/point_cloud_transport.hpp>
-
 #include <point_cloud_transport/simple_publisher_plugin.hpp>
-#include <point_cloud_interfaces/msg/compressed_point_cloud2.hpp>
 
 
 namespace projected_point_cloud_transport
@@ -58,14 +59,20 @@ class ProjectedPublisher
 
   TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override;
 
+  std::string getDataType() const override
+  {
+    return "point_cloud_interfaces/msg/ProjectedPointCloud";
+  }
+
 private:
 
-  geoemtry_msgs::msg::Pose view_point_;
+  void projectCloudOntoPlane(const sensor_msgs::msg::PointCloud2& cloud, cv::Mat& projected_pointcloud_image) const;
+
+  void projectCloudOntoSphere(const sensor_msgs::msg::PointCloud2& cloud, cv::Mat& projected_pointcloud_image) const;
 
-  // Optimization: If the user has spherical projection turned on, we never need to reallocate the
-  // spherical_projection_ image as its dimensions are not influenced by the pointcloud size.
-  cv::Mat spherical_projection_;
+  uint8_t projection_type_;
 
+  geometry_msgs::msg::Pose view_point_;
 
 };
 }  // namespace projected_point_cloud_transport

projected_point_cloud_transport/include/projected_point_cloud_transport/projected_subscriber.hpp

@@ -33,9 +33,13 @@
 #define PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_SUBSCRIBER_HPP_
 
 #include <string>
+#include <vector>
 
-#include <point_cloud_interfaces/msg/compressed_point_cloud2.hpp>
+#include <opencv2/core.hpp>
 
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <point_cloud_interfaces/msg/projected_point_cloud.hpp>
 #include <point_cloud_transport/simple_subscriber_plugin.hpp>
 #include <point_cloud_transport/transport_hints.hpp>
 
@@ -53,6 +57,18 @@ class ProjectedSubscriber
 
   DecodeResult decodeTyped(const point_cloud_interfaces::msg::ProjectedPointCloud & compressed)
   const override;
+
+  std::string getDataType() const override
+  {
+    return "point_cloud_interfaces/msg/ProjectedPointCloud";
+  }
+
+private:
+
+  void deprojectSphereToCloud(const cv::Mat& projected_pointcloud_image, const point_cloud_interfaces::msg::ProjectedPointCloud& msg, sensor_msgs::msg::PointCloud2::SharedPtr& cloud) const;
+
+  void deprojectPlaneToCloud(const cv::Mat& projected_pointcloud_image, const point_cloud_interfaces::msg::ProjectedPointCloud& msg, sensor_msgs::msg::PointCloud2::SharedPtr& cloud) const;
+
 };
 }  // namespace projected_point_cloud_transport
 

projected_point_cloud_transport/package.xml

@@ -20,10 +20,11 @@
 
   <exec_depend>pluginlib</exec_depend>
 
+  <depend>OpenCV</depend>
   <depend>point_cloud_interfaces</depend>
   <depend>point_cloud_transport</depend>
   <depend>rclcpp</depend>
-  <depend>libopencv</depend>
+  <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

projected_point_cloud_transport/src/projected_publisher.cpp

@@ -31,8 +31,10 @@
 
 #include <string>
 
+#include <opencv2/imgcodecs.hpp>
+
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <sensor_msgs/point_cloud_iterator.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
 
 #include <projected_point_cloud_transport/projected_publisher.hpp>
 
@@ -61,11 +63,11 @@ ProjectedPublisher::TypedEncodeResult ProjectedPublisher::encodeTyped(
     case(point_cloud_interfaces::msg::ProjectedPointCloud::SPHERICAL_PROJECTION):
      projectCloudOntoSphere(raw, projected_pointcloud_image);
     default:
-      RCLCPP_ERROR(getLogger(), "Projection type " << projection_type_ << " is not known/supported!");
+      RCLCPP_ERROR_STREAM(getLogger(), "Projection type " << projection_type_ << " is not known/supported!");
   }
 
   if(projected_pointcloud_image.empty()){
-    RCLCPP_ERROR(getLogger(), "Projection type " << projection_type_ << " failed to project pointcloud!");
+    RCLCPP_ERROR_STREAM(getLogger(), "Projection type " << projection_type_ << " failed to project pointcloud!");
     return cras::make_unexpected("Failed to project pointcloud onto image!");
   }
 
@@ -86,10 +88,10 @@ ProjectedPublisher::TypedEncodeResult ProjectedPublisher::encodeTyped(
   return compressed;
 }
 
-void ProjectedPublisher::projectCloudOntoPlane(const sensor_msgs::msg::PointCloud2& cloud, cv::Mat& projected_pointcloud_image){
+void ProjectedPublisher::projectCloudOntoPlane(const sensor_msgs::msg::PointCloud2& cloud, cv::Mat& projected_pointcloud_image) const{
   if(cloud.is_dense){
     // TODO ([email protected]): if the pointcloud is already organized, just point the cv::Mat at the data
-    projected_pointcloud_image = cv::Mat(cv::Size(cloud.width, cloud.height), CV_16UC1, cloud.data);
+    projected_pointcloud_image = cv::Mat((int)cloud.height, (int)cloud.width, CV_32FC3, (void*)cloud.data.data());
   }else{
     // TODO ([email protected]): Make these parameters
     const int view_height = 720;
@@ -99,11 +101,18 @@ void ProjectedPublisher::projectCloudOntoPlane(const sensor_msgs::msg::PointClou
     const float ppy = view_height/2;
     const float fx = view_width;
     const float fy = view_width;
+
     // if the pointcloud is NOT already organized, we need to apply the projection
+
+    // iterate over the pointcloud
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
+
     for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
-      const double& x = *iter_x - view_point.position.x;
-      const double& y = *iter_y - view_point.position.y;
-      const double& z = *iter_z - view_point.position.z;
+      const double& x = *iter_x - view_point_.position.x;
+      const double& y = *iter_y - view_point_.position.y;
+      const double& z = *iter_z - view_point_.position.z;
 
       if(!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)){
         continue;
@@ -112,60 +121,61 @@ void ProjectedPublisher::projectCloudOntoPlane(const sensor_msgs::msg::PointClou
       // TODO (john-maidbot): rotate the d x/y/z so that z is along the view point orientation
       // TODO: can that be made more efficient by transforming the view point instead?
 
-      if(z == 0){
-        continue;
-      }
-
       const int col = x/z * fx + ppx;
       const int row = y/z * fy + ppy;
 
+      if(z == 0 || col < 0 || row < 0 || col >= view_width || row >= view_height){
+        continue;
+      }
+
       auto& cell = projected_pointcloud_image.at<uint16_t>(row, col);
-      cell = std::min(cell, static_cast<uint16_t>(depth * 1000)); // mm resolution
+      cell = std::min(cell, static_cast<uint16_t>(z * 1000)); // mm resolution
     }    
-
   }
 }
 
-void ProjectedPublisher::projectCloudOntoSphere(const sensor_msgs::msg::PointCloud2& cloud, cv::Mat& projected_pointcloud_image){
+void ProjectedPublisher::projectCloudOntoSphere(const sensor_msgs::msg::PointCloud2& cloud, cv::Mat& projected_pointcloud_image) const{
   // TODO ([email protected]): Make these parameters
-  const double phi_resolution = 0.5; // radians
-  const double theta_resolution = 0.5; // radians
+  const double phi_resolution = 0.034; // radians
+  const double theta_resolution = 0.034; // radians
   const int phi_bins = 2 * M_PI / phi_resolution;
-  const int theta_bins = 2 * M_PI / rho_resolution;
+  const int theta_bins = 2 * M_PI / theta_resolution;
 
-  if(spherical_image.empty()){
-    spherical_image_ = cv::Mat{phi_bins, theta_bins, CV_16UC1, cv::Scalar(0)};
-  }else{
-    spherical_image.setTo(0);
-  }
+  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");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
     // TODO ([email protected]): should spherical projection account
     // for the orientation of the view point?
-    const double& x = *iter_x - view_point.position.x;
-    const double& y = *iter_y - view_point.position.y;
-    const double& z = *iter_z - view_point.position.z;
+    const double& x = *iter_x - view_point_.position.x;
+    const double& y = *iter_y - view_point_.position.y;
+    const double& z = *iter_z - view_point_.position.z;
 
     if(!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)){
       continue;
     }
 
     const double rho = std::sqrt(x * x + y * y + z * z);
-    const double phi = std::atan2(y, x);
-    const double theta = std::acos(z / rho);
+    double phi = std::atan2(y, x);
+    double theta = std::acos(z / rho);
+    if (phi < 0){
+      phi += 2*M_PI;
+    }
+    if (theta < 0){
+      theta += 2*M_PI;
+    }
 
-    const int phi_index = phi / phi_resolution;
-    const int theta_index = theta / theta_resolution;
+    const int phi_index = std::min(static_cast<int>(phi / phi_resolution), phi_bins);
+    const int theta_index = std::min(static_cast<int>(theta / theta_resolution), theta_bins);
 
-    auto& cell = spherical_image_.at<uint16_t>(phi_index, rho_index);
+    auto& cell = spherical_image.at<uint16_t>(phi_index, theta_index);
     cell = std::min(cell, static_cast<uint16_t>(rho * 1000)); // mm resolution
   }
-  projected_pointcloud_image = spherical_image_;
+  projected_pointcloud_image = spherical_image;
 }
 
 }  // namespace projected_point_cloud_transport