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; } }
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; } }
/* * Setter functions MUST be used carefully - If you arbitrarily change SDF values, it is not a proper SDF any more! * * Use of these functions can be prevented by calling SignedDistanceField::Lock() on the SDF, at which point these functions * will fail with a warning printed to std_err. */ inline bool Set(const double x, const double y, const double z, float value) { if (!locked_) { return distance_field_.SetValue(x, y, z, value); } else { std::cerr << "Attempt to set value in locked SDF" << std::endl; return false; } }
inline bool Set(const Eigen::Vector3d& location, float value) { if (!locked_) { return distance_field_.SetValue(location, value); } else { std::cerr << "Attempt to set value in locked SDF" << std::endl; return false; } }
inline bool Set(const VoxelGrid::GRID_INDEX& index, const float value) { if (!locked_) { return distance_field_.SetValue(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 float Get(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).first; }
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 std::pair<float, bool> GetSafe(const Eigen::Vector3d& location) const { return distance_field_.GetImmutable(location); }
inline std::pair<float, bool> GetSafe(const double x, const double y, const double z) const { return distance_field_.GetImmutable(x, y, z); }
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); }
inline float Get(const Eigen::Vector3d& location) const { return distance_field_.GetImmutable(location).first; }
inline std::vector<int64_t> LocationToGridIndex(const Eigen::Vector3d& location) const { return distance_field_.LocationToGridIndex(location); }
inline bool CheckInBounds(const Eigen::Vector3d& location) const { return distance_field_.GetImmutable(location.x(), location.y(), location.z()).second; }
inline std::vector<double> GridIndexToLocation(const VoxelGrid::GRID_INDEX& index) const { return distance_field_.GridIndexToLocation(index); }
inline bool CheckInBounds(const double x, const double y, const double z) const { return distance_field_.GetImmutable(x, y, z).second; }
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 = maxima_map.GridIndexToLocation(x_idx, y_idx, z_idx); const Eigen::Vector3d extrema = maxima_map.GetImmutable(x_idx, y_idx, z_idx).first; if (!std::isinf(extrema.x()) && !std::isinf(extrema.y()) && !std::isinf(extrema.z())) { const double distance = (extrema - location.block<3, 1>(0, 0)).norm(); if (distance < sdf.GetResolution()) { visualization_msgs::Marker maxima_rep; // Populate the header maxima_rep.header.frame_id = "world"; // Populate the options maxima_rep.ns = "extrema"; maxima_rep.id = (int32_t)sdf.HashDataIndex(x_idx, y_idx, z_idx); maxima_rep.action = visualization_msgs::Marker::ADD; maxima_rep.lifetime = ros::Duration(0.0); maxima_rep.frame_locked = false; maxima_rep.pose.position = EigenHelpersConversions::EigenVector4dToGeometryPoint(location); maxima_rep.pose.orientation = EigenHelpersConversions::EigenQuaterniondToGeometryQuaternion(Eigen::Quaterniond::Identity()); maxima_rep.type = visualization_msgs::Marker::SPHERE; maxima_rep.scale.x = sdf.GetResolution(); maxima_rep.scale.y = sdf.GetResolution(); maxima_rep.scale.z = sdf.GetResolution(); maxima_rep.color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(1.0, 0.5, 0.0, 1.0); display_markers.markers.push_back(maxima_rep); } } else { std::cout << "Encountered inf extrema @ (" << x_idx << "," << y_idx << "," << z_idx << ")" << std::endl; } } } } std::cout << "(0,0,0) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetGradient((int64_t)0, (int64_t)0, (int64_t)0, true)) << std::endl; std::cout << "(1,1,1) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetGradient((int64_t)1, (int64_t)1, (int64_t)1, true)) << std::endl; std::cout << "(2,2,2) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetGradient((int64_t)2, (int64_t)2, (int64_t)2, true)) << std::endl; std::cout << "(0,0,0) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetSmoothGradient((int64_t)0, (int64_t)0, (int64_t)0, res)) << std::endl; std::cout << "(1,1,1) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetSmoothGradient((int64_t)1, (int64_t)1, (int64_t)1, res)) << std::endl; std::cout << "(2,2,2) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetSmoothGradient((int64_t)2, (int64_t)2, (int64_t)2, res)) << std::endl; std::cout << "(0,0,0) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetAutoDiffGradient((int64_t)0, (int64_t)0, (int64_t)0)) << std::endl; std::cout << "(1,1,1) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetAutoDiffGradient((int64_t)1, (int64_t)1, (int64_t)1)) << std::endl; std::cout << "(2,2,2) " << PrettyPrint::PrettyPrint(virtual_border_sdf.GetAutoDiffGradient((int64_t)2, (int64_t)2, (int64_t)2)) << std::endl; std::cout << "(0,0,0) " << PrettyPrint::PrettyPrint(maxima_map.GetImmutable((int64_t)0, (int64_t)0, (int64_t)0).first) << std::endl; std::cout << "(1,1,1) " << PrettyPrint::PrettyPrint(maxima_map.GetImmutable((int64_t)1, (int64_t)1, (int64_t)1).first) << std::endl; std::cout << "(2,2,2) " << PrettyPrint::PrettyPrint(maxima_map.GetImmutable((int64_t)2, (int64_t)2, (int64_t)2).first) << std::endl; display_fn(display_markers); }
inline bool CheckInBounds(const VoxelGrid::GRID_INDEX& index) const { return distance_field_.GetImmutable(index.x, index.y, index.z).second; }