void test_voxel_grid_serialization()
{
    VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
    // Load with special values
    int check_val = 1;
    std::vector<int> check_vals;
    for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
    {
        for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
        {
            for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
            {
                test_grid.SetValue(x_index, y_index, z_index, check_val);
                check_vals.push_back(check_val);
                check_val++;
            }
        }
    }
    std::vector<uint8_t> buffer;
    VoxelGrid::VoxelGrid<int>::Serialize(test_grid, buffer, arc_utilities::SerializeFixedSizePOD<int>);
    const VoxelGrid::VoxelGrid<int> read_grid = VoxelGrid::VoxelGrid<int>::Deserialize(buffer, 0, arc_utilities::DeserializeFixedSizePOD<int>).first;
    // Check the values
    int check_index = 0;
    bool pass = true;
    for (int64_t x_index = 0; x_index < read_grid.GetNumXCells(); x_index++)
    {
        for (int64_t y_index = 0; y_index < read_grid.GetNumYCells(); y_index++)
        {
            for (int64_t z_index = 0; z_index < read_grid.GetNumZCells(); z_index++)
            {
                int ref_val = read_grid.GetImmutable(x_index, y_index, z_index).first;
                //std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
                if (ref_val == check_vals[check_index])
                {
                    //std::cout << "Check pass" << std::endl;
                }
                else
                {
                    std::cout << "Check fail" << std::endl;
                    pass = false;
                }
                check_index++;
            }
        }
    }
    if (pass)
    {
        std::cout << "VG-I de/serialize - All checks pass" << std::endl;
    }
    else
    {
        std::cout << "*** VG-I de/serialize - Checks failed ***" << std::endl;
    }
}

void test_compute_convex_segments(
    const std::function<void(
      const visualization_msgs::MarkerArray&)>& display_fn)
{
  const double res = 1.0;
  const int64_t x_size = 100;
  const int64_t y_size = 100;
  const int64_t z_size = 50;
  const Eigen::Isometry3d origin_transform
      = Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond(
          Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()));
  sdf_tools::TaggedObjectCollisionMapGrid tocmap(origin_transform, "world", res, x_size, y_size, z_size, sdf_tools::TAGGED_OBJECT_COLLISION_CELL(0.0, 0u));
  for (int64_t x_idx = 0; x_idx < tocmap.GetNumXCells(); x_idx++)
  {
    for (int64_t y_idx = 0; y_idx < tocmap.GetNumYCells(); y_idx++)
    {
      for (int64_t z_idx = 0; z_idx < tocmap.GetNumZCells(); z_idx++)
      {
        if ((x_idx < 10) || (y_idx < 10) || (x_idx >= tocmap.GetNumXCells() - 10) || (y_idx >= tocmap.GetNumYCells() - 10))
        {
          tocmap.SetValue(x_idx, y_idx, z_idx, sdf_tools::TAGGED_OBJECT_COLLISION_CELL(1.0, 1u));
        }
        else if ((x_idx >= 40) && (y_idx >= 40) && (x_idx < 60) && (y_idx < 60))
        {
          tocmap.SetValue(x_idx, y_idx, z_idx, sdf_tools::TAGGED_OBJECT_COLLISION_CELL(1.0, 2u));
        }
        if (((x_idx >= 45) && (x_idx < 55)) || ((y_idx >= 45) && (y_idx < 55)))
        {
          tocmap.SetValue(x_idx, y_idx, z_idx, sdf_tools::TAGGED_OBJECT_COLLISION_CELL(0.0, 0u));
        }
      }
    }
  }
  visualization_msgs::MarkerArray display_markers;
  visualization_msgs::Marker env_marker = tocmap.ExportForDisplay();
  env_marker.id = 1;
  env_marker.ns = "environment";
  display_markers.markers.push_back(env_marker);
  visualization_msgs::Marker components_marker = tocmap.ExportConnectedComponentsForDisplay(false);
  components_marker.id = 1;
  components_marker.ns = "environment_components";
  display_markers.markers.push_back(components_marker);
  const double connected_threshold = 1.75;
  const uint32_t number_of_convex_segments_manual_border = tocmap.UpdateConvexSegments(connected_threshold, false);
  std::cout << "Identified " << number_of_convex_segments_manual_border
            << " convex segments via SDF->maxima map->connected components (no border added)"
            << std::endl;
  for (uint32_t object_id = 0u; object_id <= 4u; object_id++)
  {
    for (uint32_t convex_segment = 1u; convex_segment <= number_of_convex_segments_manual_border; convex_segment++)
    {
      visualization_msgs::Marker segment_marker = tocmap.ExportConvexSegmentForDisplay(object_id, convex_segment);
      if (segment_marker.points.size() > 0)
      {
        segment_marker.ns += "_no_border";
        display_markers.markers.push_back(segment_marker);
      }
    }
  }
  const uint32_t number_of_convex_segments_virtual_border = tocmap.UpdateConvexSegments(connected_threshold, true);
  std::cout << "Identified " << number_of_convex_segments_virtual_border
            << " convex segments via SDF->maxima map->connected components (virtual border added)"
            << std::endl;
  for (uint32_t object_id = 0u; object_id <= 4u; object_id++)
  {
    for (uint32_t convex_segment = 1u; convex_segment <= number_of_convex_segments_virtual_border; convex_segment++)
    {
      visualization_msgs::Marker segment_marker = tocmap.ExportConvexSegmentForDisplay(object_id, convex_segment);
      if (segment_marker.points.size() > 0)
      {
        segment_marker.ns += "_virtual_border";
        display_markers.markers.push_back(segment_marker);
      }
    }
  }
  const auto sdf_result
      = tocmap.ExtractSignedDistanceField(std::numeric_limits<float>::infinity(), std::vector<uint32_t>(), true, false);
  std::cout << "(no border) SDF extrema: " << PrettyPrint::PrettyPrint(sdf_result.second) << std::endl;
  const sdf_tools::SignedDistanceField& sdf = sdf_result.first;
  visualization_msgs::Marker sdf_marker = sdf.ExportForDisplay(1.0f);
  sdf_marker.id = 1;
  sdf_marker.ns = "environment_sdf_no_border";
  display_markers.markers.push_back(sdf_marker);
  const auto virtual_border_sdf_result
      = tocmap.ExtractSignedDistanceField(std::numeric_limits<float>::infinity(), std::vector<uint32_t>(), true, true);
  std::cout << "(virtual border) SDF extrema: " << PrettyPrint::PrettyPrint(virtual_border_sdf_result.second) << std::endl;
  const sdf_tools::SignedDistanceField& virtual_border_sdf = virtual_border_sdf_result.first;
  visualization_msgs::Marker virtual_border_sdf_marker = virtual_border_sdf.ExportForDisplay(1.0f);
  virtual_border_sdf_marker.id = 1;
  virtual_border_sdf_marker.ns = "environment_sdf_virtual_border";
  display_markers.markers.push_back(virtual_border_sdf_marker);
  // Make extrema markers
  const VoxelGrid::VoxelGrid<Eigen::Vector3d> maxima_map = virtual_border_sdf.ComputeLocalExtremaMap();
  for (int64_t x_idx = 0; x_idx < maxima_map.GetNumXCells(); x_idx++)
  {
    for (int64_t y_idx = 0; y_idx < maxima_map.GetNumYCells(); y_idx++)
    {
      for (int64_t z_idx = 0; z_idx < maxima_map.GetNumZCells(); z_idx++)
      {
        const Eigen::Vector4d location
//.........这里部分代码省略.........

        inline std::pair<double, bool> EstimateDistance(const Eigen::Vector3d& location) const
        {
            const std::vector<int64_t> indices = LocationToGridIndex(location);
            if (indices.size() == 3)
            {
                const Eigen::Vector3d gradient = EigenHelpers::StdVectorDoubleToEigenVector3d(GetGradient(indices[0], indices[1], indices[2], true));
                const std::vector<double> cell_location = GridIndexToLocation(indices[0], indices[1], indices[2]);
                const Eigen::Vector3d cell_location_to_our_location(location.x() - cell_location[0], location.y() - cell_location[1], location.z() - cell_location[2]);
                const double nominal_distance = (double)distance_field_.GetImmutable(indices[0], indices[1], indices[2]).first;
                const double corrected_nominal_distance = (nominal_distance >= 0.0) ? nominal_distance - (GetResolution() * 0.5) : nominal_distance + (GetResolution() * 0.5);
                const double cell_location_to_our_location_dot_gradient = cell_location_to_our_location.dot(gradient);
                //const double gradient_dot_gradient = gradient.dot(gradient); // == squared norm of gradient
                //const Eigen::Vector3d cell_location_to_our_location_projected_on_gradient = (cell_location_to_our_location_dot_gradient / gradient.dot(gradient)) * gradient;
                //const double distance_adjustment = cell_location_to_our_location_projected_on_gradient.norm();
                const double distance_adjustment = cell_location_to_our_location_dot_gradient / gradient.norm();
                const double distance_estimate = corrected_nominal_distance + distance_adjustment;
                if ((corrected_nominal_distance >= 0.0) == (distance_estimate >= 0.0))
                {
                    return std::make_pair(distance_estimate, true);
                }
                else if (corrected_nominal_distance >= 0.0)
                {
                    const double fudge_distance = GetResolution() * 0.0625;
                    return std::make_pair(fudge_distance, true);
                }
                else
                {
                    const double fudge_distance = GetResolution() * -0.0625;
                    return std::make_pair(fudge_distance, true);
                }
//                else
//                {
//                    const double real_distance_adjustment = GetResolution() * 0.20710678118654757;
//                    const double revised_corrected_nominal_distance = (nominal_distance >= 0.0) ? nominal_distance - real_distance_adjustment : nominal_distance + real_distance_adjustment;
//                    const double revised_distance_estimate = revised_corrected_nominal_distance + distance_adjustment;
//                    return std::make_pair(revised_distance_estimate, true);
//                }
            }
            else
            {
                return std::make_pair((double)distance_field_.GetOOBValue(), false);
            }
        }

 inline bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, const float value)
 {
     if (!locked_)
     {
         return distance_field_.SetValue(x_index, y_index, z_index, value);
     }
     else
     {
         std::cerr << "Attempt to set value in locked SDF" << std::endl;
         return false;
     }
 }

 inline float GetOOBValue() const
 {
     return distance_field_.GetDefaultValue();
 }

 inline double GetResolution() const
 {
     return distance_field_.GetCellSizes()[0];
 }

 inline double GetZSize() const
 {
     return distance_field_.GetZSize();
 }

 inline bool CheckInBounds(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
 {
     return distance_field_.GetImmutable(x_index, y_index, z_index).second;
 }

 inline float Get(const double x, const double y, const double z) const
 {
     return distance_field_.GetImmutable(x, y, z).first;
 }

 inline std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
 {
     return distance_field_.GridIndexToLocation(x_index, y_index, z_index);
 }

 inline Eigen::Affine3d GetOriginTransform() const
 {
     return distance_field_.GetOriginTransform();
 }

 inline std::pair<float, bool> GetSafe(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
 {
     return distance_field_.GetImmutable(x_index, y_index, z_index);
 }

 inline int64_t GetNumZCells() const
 {
     return distance_field_.GetNumZCells();
 }

 inline std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const
 {
     return distance_field_.LocationToGridIndex(x, y, z);
 }