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);
}
}
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);
}
