inline std::vector<double> GetGradient(const int64_t x_index, const int64_t y_index, const int64_t z_index, const bool enable_edge_gradients=false) const
{
// Make sure the index is inside bounds
if ((x_index >= 0) && (y_index >= 0) && (z_index >= 0) && (x_index < GetNumXCells()) && (y_index < GetNumYCells()) && (z_index < GetNumZCells()))
{
// Make sure the index we're trying to query is one cell in from the edge
if ((x_index > 0) && (y_index > 0) && (z_index > 0) && (x_index < (GetNumXCells() - 1)) && (y_index < (GetNumYCells() - 1)) && (z_index < (GetNumZCells() - 1)))
{
double inv_twice_resolution = 1.0 / (2.0 * GetResolution());
double gx = (Get(x_index + 1, y_index, z_index) - Get(x_index - 1, y_index, z_index)) * inv_twice_resolution;
double gy = (Get(x_index, y_index + 1, z_index) - Get(x_index, y_index - 1, z_index)) * inv_twice_resolution;
double gz = (Get(x_index, y_index, z_index + 1) - Get(x_index, y_index, z_index - 1)) * inv_twice_resolution;
return std::vector<double>{gx, gy, gz};
}
// If we're on the edge, handle it specially
else if (enable_edge_gradients)
{
// Get the "best" indices we can use
int64_t low_x_index = std::max((int64_t)0, x_index - 1);
int64_t high_x_index = std::min(GetNumXCells() - 1, x_index + 1);
int64_t low_y_index = std::max((int64_t)0, y_index - 1);
int64_t high_y_index = std::min(GetNumYCells() - 1, y_index + 1);
int64_t low_z_index = std::max((int64_t)0, z_index - 1);
int64_t high_z_index = std::min(GetNumZCells() - 1, z_index + 1);
// Compute the axis increments
double x_increment = (high_x_index - low_x_index) * GetResolution();
double y_increment = (high_y_index - low_y_index) * GetResolution();
double z_increment = (high_z_index - low_z_index) * GetResolution();
// Compute the gradients for each axis - by default these are zero
double gx = 0.0;
double gy = 0.0;
double gz = 0.0;
// Only if the increments are non-zero do we compute the gradient of an axis
if (x_increment > 0.0)
{
double inv_x_increment = 1.0 / x_increment;
double high_x_value = Get(high_x_index, y_index, z_index);
double low_x_value = Get(low_x_index, y_index, z_index);
// Compute the gradient
gx = (high_x_value - low_x_value) * inv_x_increment;
}
if (y_increment > 0.0)
{
double inv_y_increment = 1.0 / y_increment;
double high_y_value = Get(x_index, high_y_index, z_index);
double low_y_value = Get(x_index, low_y_index, z_index);
// Compute the gradient
gy = (high_y_value - low_y_value) * inv_y_increment;
}
if (z_increment > 0.0)
{
double inv_z_increment = 1.0 / z_increment;
double high_z_value = Get(x_index, y_index, high_z_index);
double low_z_value = Get(x_index, y_index, low_z_index);
// Compute the gradient
gz = (high_z_value - low_z_value) * inv_z_increment;
}
// Assemble and return the computed gradient
return std::vector<double>{gx, gy, gz};
}
// Edge gradients disabled, return no gradient
else
{
return std::vector<double>();
}
}
// If we're out of bounds, return no gradient
else
{
return std::vector<double>();
}
}
void test_estimate_distance(
const std::function<void(
const visualization_msgs::MarkerArray&)>& display_fn)
{
const double res = 1.0;
const double size = 10.0;
const Eigen::Isometry3d origin_transform
= Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond(
Eigen::AngleAxisd(M_PI_4, Eigen::Vector3d::UnitZ()));
auto map = sdf_tools::CollisionMapGrid(origin_transform, "world", res, size, size, 1.0, sdf_tools::COLLISION_CELL(0.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(5.0, 5.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(5.0, 6.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(6.0, 5.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(6.0, 6.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(7.0, 7.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(2.0, 2.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(3.0, 2.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(4.0, 2.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(2.0, 3.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(2.0, 4.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
map.SetValue4d(origin_transform * Eigen::Vector4d(2.0, 7.0, 0.0, 1.0), sdf_tools::COLLISION_CELL(1.0));
const std_msgs::ColorRGBA collision_color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(1.0, 0.0, 0.0, 0.5);
const std_msgs::ColorRGBA free_color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(0.0, 0.0, 0.0, 0.0);
const std_msgs::ColorRGBA unknown_color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(0.0, 0.0, 0.0, 0.0);
const auto map_marker = map.ExportForDisplay(collision_color, free_color, unknown_color);
const auto sdf = map.ExtractSignedDistanceField(1e6, true, false).first;
const auto sdf_marker = sdf.ExportForDisplay(0.05f);
// Assemble a visualization_markers::Marker representation of the SDF to display in RViz
visualization_msgs::Marker distance_rep;
// Populate the header
distance_rep.header.frame_id = "world";
// Populate the options
distance_rep.ns = "estimated_distance_display";
distance_rep.id = 1;
distance_rep.type = visualization_msgs::Marker::CUBE_LIST;
distance_rep.action = visualization_msgs::Marker::ADD;
distance_rep.lifetime = ros::Duration(0.0);
distance_rep.frame_locked = false;
distance_rep.pose
= EigenHelpersConversions::EigenIsometry3dToGeometryPose(sdf.GetOriginTransform());
const double step = sdf.GetResolution() * 0.125 * 0.25;
distance_rep.scale.x = sdf.GetResolution() * step;// * 0.125;
distance_rep.scale.y = sdf.GetResolution() * step;// * 0.125;
distance_rep.scale.z = sdf.GetResolution() * 0.95;// * 0.125;// * 0.125;
// Add all the cells of the SDF to the message
double min_distance = 0.0;
double max_distance = 0.0;
// Add colors for all the cells of the SDF to the message
for (double x = 0; x < sdf.GetXSize(); x += step)
{
for (double y = 0; y < sdf.GetYSize(); y += step)
{
double z = 0.5;
//for (double z = 0; z <= sdf.GetZSize(); z += step)
{
const Eigen::Vector4d point(x, y, z, 1.0);
const Eigen::Vector4d point_in_grid_frame = origin_transform * point;
// Update minimum/maximum distance variables
const double distance
= sdf.EstimateDistance4d(point_in_grid_frame).first;
if (distance > max_distance)
{
max_distance = distance;
}
if (distance < min_distance)
{
min_distance = distance;
}
}
}
}
std::cout << "Min dist " << min_distance << " Max dist " << max_distance << std::endl;
for (double x = 0; x < sdf.GetXSize(); x += step)
{
for (double y = 0; y < sdf.GetYSize(); y += step)
{
double z = 0.5;
//for (double z = 0; z <= sdf.GetZSize(); z += step)
{
const Eigen::Vector4d point(x, y, z, 1.0);
const Eigen::Vector4d point_in_grid_frame = origin_transform * point;
const double distance
= sdf.EstimateDistance4d(point_in_grid_frame).first;
if (distance >= 0.0)
{
const std_msgs::ColorRGBA new_color
= arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>
::InterpolateHotToCold(distance, 0.0, max_distance);
distance_rep.colors.push_back(new_color);
}
else
{
const std_msgs::ColorRGBA new_color
= arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(1.0, 0.0, 1.0, 1.0);
distance_rep.colors.push_back(new_color);
}
geometry_msgs::Point new_point;
new_point.x = x;
new_point.y = y;
new_point.z = z;
distance_rep.points.push_back(new_point);
}
}
}
visualization_msgs::MarkerArray markers;
markers.markers = {map_marker, sdf_marker, distance_rep};
// Make gradient markers
for (int64_t x_idx = 0; x_idx < sdf.GetNumXCells(); x_idx++)
{
for (int64_t y_idx = 0; y_idx < sdf.GetNumYCells(); y_idx++)
{
for (int64_t z_idx = 0; z_idx < sdf.GetNumZCells(); z_idx++)
{
const Eigen::Vector4d location
= sdf.GridIndexToLocation(x_idx, y_idx, z_idx);
const std::vector<double> discrete_gradient
= sdf.GetGradient4d(location, true);
std::cout << "Discrete gradient " << PrettyPrint::PrettyPrint(discrete_gradient) << std::endl;
const std::vector<double> smooth_gradient
= sdf.GetSmoothGradient4d(location, sdf.GetResolution() * 0.125);
std::cout << "Smooth gradient " << PrettyPrint::PrettyPrint(smooth_gradient) << std::endl;
const std::vector<double> autodiff_gradient
= sdf.GetAutoDiffGradient4d(location);
std::cout << "Autodiff gradient " << PrettyPrint::PrettyPrint(autodiff_gradient) << std::endl;
if (discrete_gradient.size() == 3)
{
const Eigen::Vector4d gradient_vector(discrete_gradient[0],
discrete_gradient[1],
discrete_gradient[2],
0.0);
visualization_msgs::Marker gradient_rep;
// Populate the header
gradient_rep.header.frame_id = "world";
// Populate the options
gradient_rep.ns = "discrete_gradient";
gradient_rep.id = (int32_t)sdf.HashDataIndex(x_idx, y_idx, z_idx);
gradient_rep.type = visualization_msgs::Marker::ARROW;
gradient_rep.action = visualization_msgs::Marker::ADD;
gradient_rep.lifetime = ros::Duration(0.0);
gradient_rep.frame_locked = false;
gradient_rep.pose
= EigenHelpersConversions::EigenIsometry3dToGeometryPose(Eigen::Isometry3d::Identity());
gradient_rep.points.push_back(EigenHelpersConversions::EigenVector4dToGeometryPoint(location));
gradient_rep.points.push_back(EigenHelpersConversions::EigenVector4dToGeometryPoint(location + gradient_vector));
gradient_rep.scale.x = sdf.GetResolution() * 0.06125;
gradient_rep.scale.y = sdf.GetResolution() * 0.125;
gradient_rep.scale.z = 0.0;
gradient_rep.color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(1.0, 0.5, 0.0, 1.0);
markers.markers.push_back(gradient_rep);
}
if (smooth_gradient.size() == 3)
{
const Eigen::Vector4d gradient_vector(smooth_gradient[0],
smooth_gradient[1],
smooth_gradient[2],
0.0);
visualization_msgs::Marker gradient_rep;
// Populate the header
gradient_rep.header.frame_id = "world";
// Populate the options
gradient_rep.ns = "smooth_gradient";
gradient_rep.id = (int32_t)sdf.HashDataIndex(x_idx, y_idx, z_idx);
gradient_rep.type = visualization_msgs::Marker::ARROW;
gradient_rep.action = visualization_msgs::Marker::ADD;
gradient_rep.lifetime = ros::Duration(0.0);
gradient_rep.frame_locked = false;
gradient_rep.pose
= EigenHelpersConversions::EigenIsometry3dToGeometryPose(Eigen::Isometry3d::Identity());
gradient_rep.points.push_back(EigenHelpersConversions::EigenVector4dToGeometryPoint(location));
gradient_rep.points.push_back(EigenHelpersConversions::EigenVector4dToGeometryPoint(location + gradient_vector));
gradient_rep.scale.x = sdf.GetResolution() * 0.06125;
gradient_rep.scale.y = sdf.GetResolution() * 0.125;
gradient_rep.scale.z = 0.0;
gradient_rep.color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(0.0, 0.5, 1.0, 1.0);
markers.markers.push_back(gradient_rep);
}
if (autodiff_gradient.size() == 3)
{
const Eigen::Vector4d gradient_vector(autodiff_gradient[0],
autodiff_gradient[1],
autodiff_gradient[2],
0.0);
visualization_msgs::Marker gradient_rep;
// Populate the header
gradient_rep.header.frame_id = "world";
// Populate the options
gradient_rep.ns = "autodiff_gradient";
gradient_rep.id = (int32_t)sdf.HashDataIndex(x_idx, y_idx, z_idx);
gradient_rep.type = visualization_msgs::Marker::ARROW;
gradient_rep.action = visualization_msgs::Marker::ADD;
gradient_rep.lifetime = ros::Duration(0.0);
gradient_rep.frame_locked = false;
gradient_rep.pose
= EigenHelpersConversions::EigenIsometry3dToGeometryPose(Eigen::Isometry3d::Identity());
gradient_rep.points.push_back(EigenHelpersConversions::EigenVector4dToGeometryPoint(location));
gradient_rep.points.push_back(EigenHelpersConversions::EigenVector4dToGeometryPoint(location + gradient_vector));
gradient_rep.scale.x = sdf.GetResolution() * 0.06125;
gradient_rep.scale.y = sdf.GetResolution() * 0.125;
gradient_rep.scale.z = 0.0;
gradient_rep.color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(0.5, 0.0, 1.0, 1.0);
markers.markers.push_back(gradient_rep);
}
}
}
}
display_fn(markers);
}
