Error Propagation and Configurable Sensor Noise Models

[jwag]

This PR addresses #93. It modifies the way the variance for an individual point observation is computed allowing for consideration of pose uncertainty on the observation and enables specifying different noise models for different sensors.

Error Propagation:

Similar to the approach taken in the elevation_mapping ROS package, uncertainties in the pose of the sensor can now be accounted for in map updates. The major difference is that here, the map frame is assumed to be a fixed inertial frame (this assumption can be relaxed) so no map fusion step is required. The uncertainty in the sensor pose is composed of the uncertainty of the pose of the base with respect to the map frame and the uncertainty of the pose of the sensor with respect to the base. The base pose uncertainty is allowed to vary over time (presumably provided by the output of a localization system) while the base to sensor pose uncertainty is assumed to be fixed and helps to account for limits of calibration.

These uncertainties are combined with the uncertainty of the measurement in the sensor frame (based on a sensor noise model) using propagation of uncertainty which involves taking derivatives of the transformation ${}_{M}r_{MP} = C_{MB}(C_{BS}\ {}_{S}r_{SP} + {}_{B}r_{BS})+{}_{M}r_{MB}$ with respect to the the various uncertain parameters to obtain Jacobians $J$. Then these Jacobians are applied to the covariances as $J \Sigma J^\top$ to obtain the propagated uncertainty for that component. They are then all added together to obtain the propagated covariance.

Sensor Noise Models:

Currently sensor supported include a structured light sensor (SLS) and LiDAR. Additionally, the previously assumed noise model (an isotropic measurement uncertainty) proportional to the square of the distance along the z axis is supported via the SLS_old sensor type. A constant variance is also supported by the constant sensor type.

Limitations

• This has only been implemented for the base elevation_mapping_cupy code and neither the python or C++ ROS wrappers have been modified yet to account for these changes. I don't think that this will be terribly challenging, but I don't have the time to implement this currently as I am working only with the base em_cupy code.
• The assumption that the map frame is fixed is not true in the ROS implementation as the map is moved to the base position periodically. Depending on the use case, this can be accommodated by either setting Sigma_b_r_MB and Sigma_Theta_MB to zero or by adding a flag that indicates a moving base that disables the error propagation for the map to base.
• The SensorProcessor class is implemented using built in cupy functions and not ElementWise kernels. This means that it is likely slower that it could be. Although, I have made some attempts to speed up the code using profiling with Nsight Systems.