Detection outliers filtering bug

[simutisernestas]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • Foxy binary
• DDS implementation:
  • Fast-RTPS

Steps to reproduce the issue

When Detectron2 node function is fed with some inf or NaN values:

def outlier_filter(self, x, y, z, idx):
    '''simple outlier filter, assume Gaussian distribution and drop points with low probability (too far away from center)'''

produces the following error:

/usr/lib/python3/dist-packages/numpy/core/fromnumeric.py:3256: RuntimeWarning: Mean of empty slice.
  return _methods._mean(a, axis=axis, dtype=dtype,
/usr/lib/python3/dist-packages/numpy/core/_methods.py:161: RuntimeWarning: invalid value encountered in true_divide
  ret = ret.dtype.type(ret / rcount)
/usr/lib/python3/dist-packages/numpy/core/fromnumeric.py:3505: RuntimeWarning: Degrees of freedom <= 0 for slice
  return _methods._var(a, axis=axis, dtype=dtype, out=out, ddof=ddof,
/usr/lib/python3/dist-packages/numpy/core/_methods.py:185: RuntimeWarning: invalid value encountered in true_divide
  arrmean = um.true_divide(
/usr/lib/python3/dist-packages/numpy/core/_methods.py:209: RuntimeWarning: invalid value encountered in true_divide
  ret = ret.dtype.type(ret / rcount)
Traceback (most recent call last):
  File "/home/ernestas/nav2_dyn/install/detectron2_detector/lib/detectron2_detector/detectron2_node", line 33, in <module>
    sys.exit(load_entry_point('detectron2-detector', 'console_scripts', 'detectron2_node')())
  File "/home/ernestas/nav2_dyn/build/detectron2_detector/detectron2_detector/detectron2_node.py", line 238, in main
    rclpy.shutdown()
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/__init__.py", line 191, in spin
    executor.spin_once()
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 687, in spin_once
    raise handler.exception()
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/task.py", line 239, in __call__
    self._handler.send(None)
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 407, in handler
    await call_coroutine(entity, arg)
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 332, in _execute_subscription
    await await_or_execute(sub.callback, msg)
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 118, in await_or_execute
    return callback(*args)
  File "/home/ernestas/nav2_dyn/build/detectron2_detector/detectron2_detector/detectron2_node.py", line 141, in callback
    self.detect(points)
  File "/home/ernestas/nav2_dyn/build/detectron2_detector/detectron2_detector/detectron2_node.py", line 202, in detect
    detections = self.process_points(outputs, points)
  File "/home/ernestas/nav2_dyn/build/detectron2_detector/detectron2_detector/detectron2_node.py", line 169, in process_points
    idx = self.outlier_filter(x[idx], y[idx], z[idx], idx)
  File "/home/ernestas/nav2_dyn/build/detectron2_detector/detectron2_detector/detectron2_node.py", line 93, in outlier_filter
    rv = multivariate_normal(mean, cov)
  File "/home/ernestas/.local/lib/python3.8/site-packages/scipy/stats/_multivariate.py", line 365, in __call__
    return multivariate_normal_frozen(mean, cov,
  File "/home/ernestas/.local/lib/python3.8/site-packages/scipy/stats/_multivariate.py", line 743, in __init__
    self.cov_info = _PSD(self.cov, allow_singular=allow_singular)
  File "/home/ernestas/.local/lib/python3.8/site-packages/scipy/stats/_multivariate.py", line 158, in __init__
    s, u = scipy.linalg.eigh(M, lower=lower, check_finite=check_finite)
  File "/home/ernestas/.local/lib/python3.8/site-packages/scipy/linalg/decomp.py", line 445, in eigh
    a1 = _asarray_validated(a, check_finite=check_finite)
  File "/home/ernestas/.local/lib/python3.8/site-packages/scipy/_lib/_util.py", line 263, in _asarray_validated
    a = toarray(a)
  File "/usr/lib/python3/dist-packages/numpy/lib/function_base.py", line 495, in asarray_chkfinite
    raise ValueError(
ValueError: array must not contain infs or NaNs

Expected behavior

Robust handling of such cases, warning probably would be appropriate here.

Actual behavior

Node crash.

[SteveMacenski]

Thanks - got it. I think this is easier in one of 2 ways:

• PC is pre-processed to remove NaN/Infs
• If this is the only place that we look at depth values, then we could pass over Nan/Inf values while iterating to save the pre-processing step