Dehann Fourie

Refactor and multithread `upGibbsCliqueDensity`

2

Function and its performance can be significantly improved.

```julia WARNING: Method definition (::Type{IncrementalInference.CliqStateMachineContainer{BTND, T, InMemG, BT} where BT

manhattan-2020-02-01T11:23:31.578 during solve x606 in Cliq2 

New variable type `Pose2z`, Pose2 plus z offset, i.e. `[xyz theta]`. Includes implicit decision on naming z as vertical for world frame variables. I take that as okay, with no...

At some point we will need variable and solver support for homogeneous coordinates, e.g. `Position3h`

As per ManifoldsBase.jl. To replace prototype: https://github.com/JuliaRobotics/RoME.jl/blob/17b896c2cc531e3de6e69501f5317a5dbdc849fe/src/variables/Local_Manifold_Workaround.jl#L14-L16

fixing csm on multiple features test

3

EDIT: better to consolidate this effort with: - https://github.com/JuliaRobotics/SensorFeatureTracking.jl/issues/29

Avoid name conflict with Julia Geometry packages's `Point2` / `Point2f` etc. Same for `Point3`

Manifolds and Rotations have a name conflict on Rotations. Lets give Manifolds priority

Deferring known issues seen in test from #517. Anecdotal error that occurred during local testing. This issue happens sporadically. Will harden these new tests (TDD) once the following known issues...