Cartographer 3D behaving very weirdly

Hi!

For a while now I have been trying to get the google cartographer 3D map with no ros/anything. I am using Unitree 3D lidar. At the moment the way I am getting my map is with this function:

std::vector<std::vector<float>> CartographerModule3D::paintMap() {
  pthread_mutex_lock(&mutex);

  std::vector<std::vector<float>> point_cloud;
  auto submapList = map_builder->pose_graph()->GetAllSubmapData();
  //std::cout << "submap count: " << submapList.size() << std::endl;

  for (const auto &sub : map_builder->pose_graph()->GetAllSubmapData()) {
    const auto &submap_proto = sub.data.submap->ToProto(true).submap_3d();
    const auto &hybrid_grid = submap_proto.high_resolution_hybrid_grid();
    //const auto vecPos = CreatePointPointCloudFromHybridGrid(hybrid_grid, 0.0);

    double min_probability = 0;
    double resolution = hybrid_grid.resolution();
    std::cout << "resolution: " << resolution << std::endl;
    for (int i = 0; i < hybrid_grid.x_indices_size(); i++) {
      //int value = hybrid_grid.values(i);

      int x, y, z;
      x = hybrid_grid.x_indices(i);
      y = hybrid_grid.y_indices(i);
      z = hybrid_grid.z_indices(i);

      // Transform the cell indices to an actual voxel center point
      Eigen::Vector3f point = Eigen::Vector3f(x * resolution + resolution / 2,
                                              y * resolution + resolution / 2,
                                              z * resolution + resolution / 2);

      int prob_int = hybrid_grid.values(i);
      point_cloud.push_back({point.x(), point.y(), point.z(),
                             static_cast<float>(prob_int / 32767.0)});

      /*if (value > 32767 * min_probability) {

      }*/
    }
  }

  pthread_mutex_unlock(&mutex);
  return point_cloud;
};

This is the before cartographer pointcloud: https://streamable.com/l80io7

however the output is extremely weird since it just does not make sense at all: https://streamable.com/u5xm4n

My configurations are:

trajectory_builder_3d

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

MAX_3D_RANGE = 60.
INTENSITY_THRESHOLD = 40

TRAJECTORY_BUILDER_3D = {
  min_range = 0,
  max_range = MAX_3D_RANGE,
  num_accumulated_range_data = 1,
  voxel_filter_size = 0.1,

  high_resolution_adaptive_voxel_filter = {
    max_length = 2.,
    min_num_points = 150,
    max_range = 15.,
  },

  low_resolution_adaptive_voxel_filter = {
    max_length = 4.,
    min_num_points = 200,
    max_range = MAX_3D_RANGE,
  },

  use_online_correlative_scan_matching = true,
  real_time_correlative_scan_matcher = {
    linear_search_window = 0.15,
    angular_search_window = math.rad(1.),
    translation_delta_cost_weight = 1e-1,
    rotation_delta_cost_weight = 1e-1,
  },

  ceres_scan_matcher = {
    occupied_space_weight_0 = 1.,
    occupied_space_weight_1 = 6.,
    intensity_cost_function_options_0 = {
        weight = 0.5,
        huber_scale = 0.3,
        intensity_threshold = INTENSITY_THRESHOLD,
    },
    translation_weight = 5.,
    rotation_weight = 4e2,
    only_optimize_yaw = false,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 12,
      num_threads = 1,
    },
  },

  motion_filter = {
    max_time_seconds = 0.5,
    max_distance_meters = 0.1,
    max_angle_radians = 0.004,
  },

  rotational_histogram_size = 120,

  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
  imu_gravity_time_constant = 10.,
  pose_extrapolator = {
    use_imu_based = false,
    constant_velocity = {
      imu_gravity_time_constant = 10.,
      pose_queue_duration = 0.001,
    },
    -- TODO(wohe): Tune these parameters on the example datasets.
    imu_based = {
      pose_queue_duration = 5.,
      gravity_constant = 9.806,
      pose_translation_weight = 1.,
      pose_rotation_weight = 1.,
      imu_acceleration_weight = 1.,
      imu_rotation_weight = 1.,
      odometry_translation_weight = 1.,
      odometry_rotation_weight = 1.,
      solver_options = {
        use_nonmonotonic_steps = false;
        max_num_iterations = 10;
        num_threads = 1;
      },
    },
  },

  submaps = {
    high_resolution = 0.1,
    high_resolution_max_range = 20.,
    low_resolution = 0.45,
    num_range_data = 160,
    range_data_inserter = {
      hit_probability = 0.55,
      miss_probability = 0.49,
      num_free_space_voxels = 2,
      intensity_threshold = INTENSITY_THRESHOLD,
    },
  },

  -- When setting use_intensities to true, the intensity_cost_function_options_0
  -- parameter in ceres_scan_matcher has to be set up as well or otherwise
  -- CeresScanMatcher will CHECK-fail.
  use_intensities = false,
}

Not really sure what I am doing wrong. Help?

cartographer-project / cartographer

Cartographer 3D behaving very weirdly #1961