[ROS] Implemented Camera Renderer

panoptic_mapping_msgs/srv/RenderCameraImage.srv

@@ -0,0 +1,19 @@
+# Flag if depth image is provided that can be used for the lookup
+bool use_depth
+# Flag if custom tf is provided for the map lookup.
+# If false, transform will be calculated using tf lookups from the sensor frame
+bool use_provided_tf
+# Depth image <Optional>
+sensor_msgs/Image depth
+# Transform <Optional>
+geometry_msgs/Transform T_C_M
+# Name of the sensor frame
+string sensor_frame
+# Uncertainty Method that should be used. Choose from: <'uncertainty', 'voxel_entropy', 'voxel_probability'>
+string uncertainty_method
+# Timestamp for when the tf should be looked up <optional>
+time lookup_ts
+---
+sensor_msgs/Image class_image
+sensor_msgs/Image uncertainty_image
+sensor_msgs/Image depth_image

panoptic_mapping_ros/CMakeLists.txt

@@ -17,6 +17,7 @@ cs_add_library(${PROJECT_NAME}
         src/visualization/single_tsdf_visualizer.cpp
         src/visualization/planning_visualizer.cpp
         src/visualization/tracking_visualizer.cpp
+        src/visualization/camera_renderer.cpp
         src/conversions/conversions.cpp
         )
 

panoptic_mapping_ros/include/panoptic_mapping_ros/panoptic_mapper.h

@@ -25,6 +25,7 @@
 #include <std_srvs/Empty.h>
 
 #include "panoptic_mapping_ros/input/input_synchronizer.h"
+#include "panoptic_mapping_ros/visualization/camera_renderer.h"
 #include "panoptic_mapping_ros/visualization/planning_visualizer.h"
 #include "panoptic_mapping_ros/visualization/submap_visualizer.h"
 #include "panoptic_mapping_ros/visualization/tracking_visualizer.h"
@@ -153,6 +154,7 @@ class PanopticMapper {
   std::unique_ptr<SubmapVisualizer> submap_visualizer_;
   std::unique_ptr<PlanningVisualizer> planning_visualizer_;
   std::unique_ptr<TrackingVisualizer> tracking_visualizer_;
+  std::unique_ptr<CameraRenderer> camera_renderer_;
 
   // Which processing to perform.
   bool compute_vertex_map_ = false;

panoptic_mapping_ros/include/panoptic_mapping_ros/visualization/camera_renderer.h

@@ -0,0 +1,109 @@
+#ifndef PANOPTIC_MAPPING_ROS_VISUALIZATION_CAMERA_RENDERER_H_
+#define PANOPTIC_MAPPING_ROS_VISUALIZATION_CAMERA_RENDERER_H_
+
+#include <memory>
+#include <string>
+
+#include <panoptic_mapping/common/common.h>
+#include <panoptic_mapping/common/globals.h>
+#include <panoptic_mapping/map/submap_collection.h>
+#include <panoptic_mapping/tools/planning_interface.h>
+#include <ros/node_handle.h>
+#include <tf/transform_listener.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include "panoptic_mapping_msgs/RenderCameraImage.h"
+
+namespace panoptic_mapping {
+
+class CameraRenderer {
+ public:
+  // config
+  struct Config : public config_utilities::Config<Config> {
+    int verbosity = 1;
+    std::string ros_namespace;
+
+    Config() { setConfigName("CameraRenderer"); }
+
+   protected:
+    void setupParamsAndPrinting() override;
+    void fromRosParam() override;
+    void checkParams() const override;
+  };
+  // Uncertainty methods
+  enum UNCERTAINTY_METHOD { UNCERTAINTY, VOXEL_ENTROPY, PROBABILITY };
+
+  // Constructors.
+  explicit CameraRenderer(const Config& config,
+                          std::shared_ptr<Globals> globals,
+                          std::shared_ptr<Camera> camera,
+                          std::shared_ptr<SubmapCollection> submaps,
+                          bool print_config = true);
+  virtual ~CameraRenderer() = default;
+
+  // Interaction.
+  void setGlobalFrameName(const std::string& frame_name) {
+    global_frame_name_ = frame_name;
+  }
+
+ private:
+  const Config config_;
+
+  // Members.
+  std::shared_ptr<Globals> globals_;
+  std::shared_ptr<SubmapCollection> submaps_;
+  std::shared_ptr<Camera> camera_;
+
+  // Data.
+  std::string global_frame_name_;
+
+  // Publishers.
+  ros::NodeHandle nh_;
+  ros::ServiceServer render_camera_view_srv_;
+  tf::TransformListener tf_listener_;
+
+  // Service Callbacks
+  bool renderCameraViewCallback(
+      panoptic_mapping_msgs::RenderCameraImage::Request& request,
+      panoptic_mapping_msgs::RenderCameraImage::Response& response);
+
+  // Internal methods
+  /**
+   * Projects the camera view for pose T_M_C into a semantic segmentation image,
+   * depth image and uncertainty image This method assumes the depth_image to be
+   * a valid depth image that is used to lookup coordinates for given voxels. If
+   * this is not available use 'renderImageForPose' which also calculates the
+   * depth image
+   *
+   * @param T_M_C Transform from camera to world
+   * @param rendered_image Image for which the visible classes are rendered
+   * @param uncertainty_image Image for which the uncertainty values are stored
+   * @param depth_image Image for which the current depth of the pose is stored
+   * @param uncertainty_method Which uncertainty method to use
+   * <uncertainty|voxel_entropy|voxel_probability>
+   */
+  void renderImageForPoseAndDepth(const Transformation& T_M_C,
+                                  cv::Mat* rendered_image,
+                                  cv::Mat* uncertainty_image,
+                                  const cv::Mat* depth_image,
+                                  UNCERTAINTY_METHOD uncertainty_method) const;
+  /**
+   * Projects the camera view for pose T_M_C into a semantic segmentation image,
+   * depth image and uncertainty image This functions does not need a valid
+   * depth Image and fills up the depth image with voxel centers.
+   *
+   * @param T_M_C Transform from camera to world
+   * @param rendered_image Image for which the visible classes are rendered
+   * @param uncertainty_image Image for which the uncertainty values are stored
+   * @param depth_image Image for which the current depth of the pose is stored
+   * @param uncertainty_method Which uncertainty method to use
+   * <uncertainty|voxel_entropy|voxel_probability>
+   */
+  void renderImageForPose(const Transformation& T_M_C, cv::Mat* rendered_image,
+                          cv::Mat* uncertainty_image, cv::Mat* depth_image,
+                          UNCERTAINTY_METHOD uncertainty_method) const;
+};
+
+}  // namespace panoptic_mapping
+
+#endif  // PANOPTIC_MAPPING_ROS_VISUALIZATION_CAMERA_RENDERER_H_

panoptic_mapping_ros/src/panoptic_mapper.cpp

@@ -106,6 +106,12 @@ void PanopticMapper::setupMembers() {
       planning_interface_);
   planning_visualizer_->setGlobalFrameName(config_.global_frame_name);
 
+  // Camera Visualizer
+  camera_renderer_ = std::make_unique<CameraRenderer>(
+          config_utilities::getConfigFromRos<CameraRenderer::Config>(
+                  ros::NodeHandle(visualization_nh, "camera_renderer")),
+          globals_, camera, submaps_);
+
   // Tracking.
   tracking_visualizer_ = std::make_unique<TrackingVisualizer>(
       config_utilities::getConfigFromRos<TrackingVisualizer::Config>(