本文整理汇总了C++中voxelgrid::VoxelGrid::GridIndexToLocation方法的典型用法代码示例。如果您正苦于以下问题：C++ VoxelGrid::GridIndexToLocation方法的具体用法？C++ VoxelGrid::GridIndexToLocation怎么用？C++ VoxelGrid::GridIndexToLocation使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类voxelgrid::VoxelGrid的用法示例。
在下文中一共展示了VoxelGrid::GridIndexToLocation方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: GridIndexToLocation
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);
}示例2: test_compute_convex_segments
//.........这里部分代码省略.........
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);
}