Dense Integration Refactoring - MaplabServer Service Call to Retrieve Dense Map In Range - Some Server Improvements

mfehr commented 4 years ago

Depends on / contains:

242
249

Wait with the review until these are merged

Description

Last of the big ones, hopefully. Started as adding a dense map lookup service call in the maplab server, such that a robot can retrieve the dense map in a specific area. Turned into a improvement/refactoring of the depth integrator, unifying the duplicate implementation of things for depth maps and point clouds. Also adds the option to pass in different integration functions and an optional resource selection to determine which resources you actually want to integrate over (this is checked before loading them, so should be reasonably fast). And finally, I slapped all of the other changes into the maplab server.

Changes

Refactoring and improvement of depth integration. Now optionally takes a selection function and is templated on the integration function type, hence extending the depth integrator with different interfaces for integration functions is now easier and the depth datatypes are converted for you depending on which integration function you use. Implemented are:

typedef std::function<void(
   const voxblox::Transformation& /*T_G_S*/,
   const voxblox::Pointcloud& /*points_S*/, const voxblox::Colors& /*colors*/)>
   IntegrationFunctionPointCloudVoxblox;

typedef std::function<void(
   const aslam::Transformation /*T_G_S*/&,
   const resources::PointCloud& /*points_S*/)>
   IntegrationFunctionPointCloudMaplab;

// NOTE: this one only supports being called with depth map resource types, as 
// without providing a camera for each point cloud sensor a depth map cannot be 
// automatically created from a point cloud. The other direction works however, the 
// integration functions above can be called based on depth map resources and the
// data is automatically converted.
typedef std::function<void(
   const aslam::Transformation& /*T_G_C*/, const aslam::Camera& /*camera*/,
   const cv::Mat& /*depth_image*/, const cv::Mat& /*intensity_image*/)>
   IntegrationFunctionDepthImage;

How to use it:

#include <depth-integration/depth-integration.h>

 depth_integration::IntegrationFunctionPointCloudMaplab integration_function =
      [](
          const aslam::Transformation& /*T_G_S*/,
          const resources::PointCloud& points_C) {
       // Do whatever with the resource
      };

depth_integration::ResourceSelectionFunction one_function_to_judge_them_all =
      [](
          const int64_t /*timestamp_ns*/,
          const aslam::Transformation& /*T_G_S*/) {
        // Decide if you want to load this resource and get the integration
        // function  called based on timestamp and sensor position.
        return true;
      };

// ...

// Launch integration, chose a resource type and a set of missions.
// The selection function is optional and can be a nullptr or missing.
depth_integration::integrateAllDepthResourcesOfType(
      mission_ids, backend::ResourceType::kPointCloudXYZI,
      false /*use_undistorted_camera_for_depth_maps*/, vi_map,
      integration_function, one_function_to_judge_them_all);

Adds proper unit tests for depth integration
Adds service call to maplab server to retrieve dense map within a range around a query point Service definition: GetDenseMapInRange.srv
Adds the dense constraints call into the merging thread of the maplab server
Some more fine-grained locking in maplab_server
Update param file for maplab server to be the same as for project alice