Rework CSM using Density-Aware Sampling

[elvout]

Additions

models::Observations class

This class fuses and replaces particle_filter::Observation, particle_filter::ParticleFilter::DensitySampledPointCloud, and models::PointsFromScan to make point cloud generation and density-aware observation sampling more convenient across subsystem namespaces.

The particle filter and correlative scan matching-based SLAM have been reworked to use models::Observations instead of raw sensor_msgs::LaserScan messages. There did not seem to be a significant runtime difference for the particle filter nor CSM SLAM.

::models API Changes

• models::kLaserLoc has been renamed to models::kLaserOffset to more accurately describe its purpose.
• models::PointsFromScan() has been replaced with models::Observations::point_cloud()
• models::EvalLogSensorModel() no longer performs a range validity check; this is now the responsibility of the caller. Callers were already performing this check, so we're removing unnecessary redundancy.

Other CSM Changes

• Fixed segmentation fault in async tasks (395f9e3).
• Observations are now sampled prior to evaluating BeliefCubes. BeliefCube::eval_range now uses all observations provided to it.

Results

Our CSM implementation is essentially only sensor model-based (in reality the motion model is used as a very small tiebreaker). Because of this, we get similar improvements as in the particle filter's sensor model with regard to increased correction for distant observations, particularly when the robot is moving backwards.

Systematic Sampling	Density-Aware Sampling

Clearly there is still room for improvement, especially with angular drift, but the system is no longer catastrophically failing to localize with backwards movement.

Closes #83

[elvout]

CSM SLAM is now slower (~30% per epoch) due to the additional overhead of density-aware sampling; further optimization may be necessary to run smoothly in RT.

This may be due to sampling occurring in eval_range. Try passing in a sampled Observations instance instead.

[elvout]

CSM SLAM is now slower (~30% per epoch) due to the additional overhead of density-aware sampling; further optimization may be necessary to run smoothly in RT.

This may be due to sampling occurring in eval_range. Try passing in a sampled Observations instance instead.

Runtime difference fixed (mostly) by 553f16a. Density-aware sampling still tends to be around ~10% slower but since this is only 3-4 ms it may be in the margin of error. Sampling tends to run in <1ms, but there may be additional overhead now due to pointer indirection.

I didn't run any actual benchmarking tool so my results might just be bad. Either way we're still running in RT.