Skip to content

Commit

Permalink
Simplify result processing
Browse files Browse the repository at this point in the history 
Signed-off-by: Aleksander Kamiński <[email protected]>
  • Loading branch information
@alek-kam-robotec-ai
alek-kam-robotec-ai committed Aug 7, 2024
1 parent eacf107 commit dbb932d
Show file tree
Hide file tree
Showing 4 changed files with 38 additions and 53 deletions.
71 changes: 25 additions & 46 deletions Code/Source/Lidar/LidarRaycaster.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,18 +77,13 @@ namespace RGL
m_graph.ConfigureRayPosesNode(rglRayTransforms);
}

void LidarRaycaster::ConfigureRayRange(float range)
void LidarRaycaster::ConfigureRayRange(ROS2::Range range)
{
ValidateRayRange(range);
m_range.second = range;
// We set the graph-side value of min range to zero to be able to distinguish rays below min range from the ones above max range.
m_graph.ConfigureRayRangesNode(0.0f, m_range.second);
}
m_range = range;

void LidarRaycaster::ConfigureMinimumRayRange(float range)
{
m_range.first = range;
// We omit updating the graph-side value of min range to be able to distinguish rays below min range from the ones above max range.
UpdateNonHitValues();

m_graph.ConfigureRayRangesNode(range.m_min, range.m_max);
}

void LidarRaycaster::ConfigureRaycastResultFlags(ROS2::RaycastResultFlags flags)
Expand Down Expand Up @@ -123,6 +118,7 @@ namespace RGL

ROS2::RaycastResult LidarRaycaster::PerformRaycast(const AZ::Transform& lidarTransform)
{
AZ_Assert(m_range.has_value(), "Programmer error. Raycaster range not configured.");
RGLInterface::Get()->UpdateScene();

const AZ::Matrix3x4 lidarPose = AZ::Matrix3x4::CreateFromTransform(lidarTransform);
Expand Down Expand Up @@ -155,54 +151,21 @@ namespace RGL
m_raycastResults.m_ranges.resize(m_rglRaycastResults.m_distance.size());
}

size_t usedPointIndex = 0LU;
const size_t resultsSize = pointsExpected ? m_raycastResults.m_points.size() : m_raycastResults.m_ranges.size();
const float maxRange = m_isMaxRangeEnabled ? m_range.second : AZStd::numeric_limits<float>::infinity();
for (size_t resultIndex = 0LU; resultIndex < resultsSize; ++resultIndex)
{
m_raycastResults.m_intensities[resultIndex] = m_rglRaycastResults.m_intensity[resultIndex];
if (pointsExpected)
{
const bool isHit = m_graph.IsCompactEnabled() || aznumeric_cast<bool>(m_rglRaycastResults.m_isHit[resultIndex]);
if (isHit)
{
m_raycastResults.m_points[usedPointIndex] = Utils::AzVector3FromRglVec3f(m_rglRaycastResults.m_xyz[resultIndex]);
m_raycastResults.m_intensities[usedPointIndex] = m_rglRaycastResults.m_intensity[resultIndex];
}
else if (m_isMaxRangeEnabled)
{
const AZ::Vector4 maxVector = lidarPose * m_rayTransforms[resultIndex] * AZ::Vector4(0.0f, 0.0f, m_range.second, 1.0f);
m_raycastResults.m_points[usedPointIndex] = maxVector.GetAsVector3();
}

if (isHit || m_isMaxRangeEnabled)
{
++usedPointIndex;
}
m_raycastResults.m_points[resultIndex] = Utils::AzVector3FromRglVec3f(m_rglRaycastResults.m_xyz[resultIndex]);
}

if (distanceExpected)
{
float distance = m_rglRaycastResults.m_distance[resultIndex];
if (distance < m_range.first)
{
distance = -AZStd::numeric_limits<float>::infinity();
}
else if (distance > m_range.second)
{
distance = maxRange;
}

m_raycastResults.m_ranges[resultIndex] = distance;
m_raycastResults.m_intensities[resultIndex] = m_rglRaycastResults.m_intensity[resultIndex];
m_raycastResults.m_ranges[resultIndex] = m_rglRaycastResults.m_distance[resultIndex];
}
}

if (pointsExpected)
{
m_raycastResults.m_points.resize(usedPointIndex);
m_raycastResults.m_intensities.resize(usedPointIndex);
}

return m_raycastResults;
}

Expand All @@ -222,10 +185,26 @@ namespace RGL
}
}

void LidarRaycaster::UpdateNonHitValues()
{
float minRangeNonHitValue = -AZStd::numeric_limits<float>::infinity();
float maxRangeNonHitValue = AZStd::numeric_limits<float>::infinity();

if (m_isMaxRangeEnabled && m_range.has_value())
{
minRangeNonHitValue = m_range.value().m_min;
maxRangeNonHitValue = m_range.value().m_max;
}

m_graph.ConfigureRaytraceNodeNonHits(minRangeNonHitValue, maxRangeNonHitValue);
}

void LidarRaycaster::ConfigureMaxRangePointAddition(bool addMaxRangePoints)
{
m_isMaxRangeEnabled = addMaxRangePoints;

UpdateNonHitValues();

// We need to configure if points should be compacted to minimize the CPU operations when retrieving raycast results.
m_graph.SetIsCompactEnabled(ShouldEnableCompact());
m_graph.SetIsPcPublishingEnabled(ShouldEnablePcPublishing());
Expand Down
6 changes: 3 additions & 3 deletions Code/Source/Lidar/LidarRaycaster.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,7 @@ namespace RGL
protected:
// LidarRaycasterRequestBus overrides
void ConfigureRayOrientations(const AZStd::vector<AZ::Vector3>& orientations) override;
void ConfigureRayRange(float range) override;
void ConfigureMinimumRayRange(float range) override;
void ConfigureRayRange(ROS2::Range range) override;
void ConfigureRaycastResultFlags(ROS2::RaycastResultFlags flags) override;
bool CanHandlePublishing() override;

Expand All @@ -44,6 +43,7 @@ namespace RGL
[[maybe_unused]] float distanceNoiseStdDevBase,
[[maybe_unused]] float distanceNoiseStdDevRisePerMeter) override;
void ExcludeEntities(const AZStd::vector<AZ::EntityId>& excludedEntities) override;
void UpdateNonHitValues();
void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override;

void ConfigurePointCloudPublisher(
Expand All @@ -59,7 +59,7 @@ namespace RGL
bool m_isMaxRangeEnabled{ false }; //!< Determines whether max range point addition is enabled.
ROS2::RaycastResultFlags m_resultFlags{ ROS2::RaycastResultFlags::Points };

AZStd::pair<float, float> m_range{ 0.0f, 1.0f };
AZStd::optional<ROS2::Range> m_range{};
AZStd::vector<AZ::Matrix3x4> m_rayTransforms{ AZ::Matrix3x4::CreateIdentity() };

PipelineGraph::RaycastResults m_rglRaycastResults;
Expand Down
11 changes: 8 additions & 3 deletions Code/Source/Lidar/PipelineGraph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ namespace RGL
{
PipelineGraph::PipelineGraph()
{
ConfigureRayPosesNode({Utils::IdentityTransform});
ConfigureRayPosesNode({ Utils::IdentityTransform });
ConfigureRayRangesNode(0.0f, 1.0f);
ConfigureLidarTransformNode(AZ::Matrix3x4::CreateIdentity());
RGL_CHECK(rgl_node_raytrace(&m_nodes.m_rayTrace, nullptr));
Expand Down Expand Up @@ -91,9 +91,9 @@ namespace RGL
RGL_CHECK(rgl_node_rays_from_mat3x4f(&m_nodes.m_rayPoses, rayPoses.data(), aznumeric_cast<int32_t>(rayPoses.size())));
}

void PipelineGraph::ConfigureRayRangesNode(float minRange, float maxRange)
void PipelineGraph::ConfigureRayRangesNode(float min, float max)
{
rgl_vec2f range = { .value = { minRange, maxRange } };
const rgl_vec2f range = { .value = { min, max } };
RGL_CHECK(rgl_node_rays_set_range(&m_nodes.m_rayRanges, &range, 1));
}

Expand Down Expand Up @@ -148,6 +148,11 @@ namespace RGL
}
}

void PipelineGraph::ConfigureRaytraceNodeNonHits(float minRangeNonHitValue, float maxRangeNonHitValue)
{
RGL_CHECK(rgl_node_raytrace_configure_non_hits(m_nodes.m_rayTrace, minRangeNonHitValue, maxRangeNonHitValue));
}

void PipelineGraph::SetIsCompactEnabled(bool value)
{
SetIsFeatureEnabled(PipelineFeatureFlags::PointsCompact, value);
Expand Down
3 changes: 2 additions & 1 deletion Code/Source/Lidar/PipelineGraph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,13 +62,14 @@ namespace RGL
}

void ConfigureRayPosesNode(const AZStd::vector<rgl_mat3x4f>& rayPoses);
void ConfigureRayRangesNode(float minRange, float maxRange);
void ConfigureRayRangesNode(float min, float max);
void ConfigureYieldNodes(const rgl_field_t* fields, size_t size);
void ConfigureLidarTransformNode(const AZ::Matrix3x4& lidarTransform);
void ConfigurePcTransformNode(const AZ::Matrix3x4& pcTransform);
void ConfigureAngularNoiseNode(float angularNoiseStdDev);
void ConfigureDistanceNoiseNode(float distanceNoiseStdDevBase, float distanceNoiseStdDevRisePerMeter);
void ConfigurePcPublisherNode(const AZStd::string& topicName, const AZStd::string& frameId, const ROS2::QoS& qosPolicy);
void ConfigureRaytraceNodeNonHits(float minRangeNonHitValue, float maxRangeNonHitValue);

void SetIsCompactEnabled(bool value);
void SetIsPcPublishingEnabled(bool value);
Expand Down

0 comments on commit dbb932d

Please sign in to comment.