Implement Camera-Lidar sensor model

[nickcharron]

We want to add constraints between camera keypoints and lidar maps.

We can most likely use the singleton LidarMap class in beam_slam_common, but we will likely need a way to pass the keypoints from the VIO to this sensor model. We can simply making a topic that the VIO publishes to, then this sensor model will just subscribe to that topic and also get an instance of the lidar map. We could investigate other ways to do this without having to publish the keypoints, (such as using the graph) but it also likely won't be much overhead and the lidar maps lag behind the vision anyways so it's not bad if there's a bit of a delay.

This was my idea for doing the correspondence search:

• For each keyframe, do:
  • For each scan in LidarMap, do:
    • For each point in scan, do:
      • Project point into camera frame
      • Store point if it lands within FOV, and its associated timestamp
  • For each keypoint, do:
    • Find nearest 3 neighbors in lidar map
    • Consistency check: check that there's no occlusion issues (see LVI SAM or SuperOdometry - can't remember which one)
    • Take closest point as correspondence
    • Take 3 points as corresponding plane
    • Add point to plane constraint, or optionally point to point, or point to line

[jakemclaughlin6]

1. Subscribe to visual measurements
2. For each visual measurement, try to get the pose from the graph for it (wait until this can be achieved)
3. Once it the pose can be retrieved, transform the current lidar map into frame
4. Add point to plane constraints between visual landmarks and lidar cloud
   a. project lidar into image, for each landmark get nearest 3 points, compute plane